package main

import (
	"errors"
	"fmt"
	"github.com/op/go-logging"
	"golang.org/x/exp/io/i2c"
	"math"
	"time"
)

const (
	MODE1         byte = 0x00
	MODE2         byte = 0x01
	PRESCALE      byte = 0xFE
	LED0_ON_L     byte = 0x06
	LED0_ON_H     byte = 0x07
	LED0_OFF_L    byte = 0x08
	LED0_OFF_H    byte = 0x09
	ALL_LED_ON_L  byte = 0xFA
	ALL_LED_ON_H  byte = 0xFB
	ALL_LED_OFF_L byte = 0xFC
	ALL_LED_OFF_H byte = 0xFD
	OUTDRV        byte = 0x04
	SLEEP         byte = 0x10
	WORD          byte = 0xFF
)

type PCA9685 struct {
	i2cBus    *i2c.Device
	name      string
	initiated bool
	minPulse  uint16
	maxPulse  uint16
	log       *logging.Logger
	frequency float32
}

type Pwm struct {
	pca       *PCA9685
	pin       int
	lastValue float32
}

func createPCA9685(i2cDevice *i2c.Device, name string, minPulse uint16, maxPulse uint16, log *logging.Logger) *PCA9685 {
	log.Info(fmt.Sprintf("Creating a new PCA9685 device. Alias: %v", name))

	return &PCA9685{
		i2cBus:    i2cDevice,
		name:      name,
		initiated: false,
		minPulse:  minPulse,
		maxPulse:  maxPulse,
		log:       log,
		frequency: 1000.0,
	}
}

func (p *PCA9685) NewPwm(pin int) *Pwm {
	p.log.Info(fmt.Sprintf("Creating a new Pwm controler at pin %v from %v", pin, p.name))

	return &Pwm{
		pca:       p,
		pin:       pin,
		lastValue: 0.0,
	}
}

func (p *PCA9685) Init() {
	if p.initiated {
		p.log.Warning(fmt.Sprintf("Device \"%v\" already initiated!", p.name))
	} else {
		p.log.Info(fmt.Sprintf("Initiating \"%v\" PCA9685 device", p.name))

		p.setAllPwm(0, 0)
		p.i2cBus.WriteReg(MODE2, []byte{OUTDRV})

		time.Sleep(5 * time.Millisecond)

		var mode1 byte
		err := p.i2cBus.ReadReg(MODE1, []byte{mode1})

		if err != nil {
			p.log.Error("Can't read!")
			return
		}

		mode1 &= WORD
		mode1 = mode1 & ^SLEEP

		p.i2cBus.WriteReg(MODE1, []byte{mode1 & 0xFF})

		time.Sleep(5 * time.Millisecond)

		p.setPwmFreq(p.frequency)

		p.initiated = true
	}
}

func (p *PCA9685) SwitchOn(pwm []int) error {
	if !p.initiated {
		return errors.New(fmt.Sprintf("Device \"%v\"is not initiated!", p.name))
	}

	for i := 0; i < len(pwm); i++ {
		p.log.Info(fmt.Sprintf("Swiching on pwm #%v", pwm[i]))
		p.setPwm(pwm[i], p.minPulse, p.maxPulse)
	}

	return nil
}

func (p *PCA9685) SwitchOff(pwm []int) error {
	if !p.initiated {
		return errors.New(fmt.Sprintf("Device \"%v\"is not initiated!", p.name))
	}

	for i := 0; i < len(pwm); i++ {
		p.log.Info(fmt.Sprintf("Swiching off pwm #%v", pwm[i]))
		p.setPwm(pwm[i], 0, p.minPulse)
	}

	return nil
}

func (p *PCA9685) setPwmFreq(freqHz float32) {
	var prescaleValue float32 = 25000000.0 // 25MHz
	prescaleValue /= 4096.0
	prescaleValue /= freqHz
	prescaleValue -= 1.0

	p.log.Debug(fmt.Sprintf("Setting PWM frequency to %v Hz", freqHz))
	p.log.Debug(fmt.Sprintf("Esimated pre-scale: %v", prescaleValue))

	prescale := int(math.Floor(float64(prescaleValue + 0.5)))

	p.log.Debug(fmt.Sprintf("Final pre.scale: %v", prescale))

	var oldMode byte
	err := p.i2cBus.ReadReg(MODE1, []byte{oldMode})

	if err != nil {
		p.log.Error("Can't read!")
	}

	oldMode &= WORD

	newMode := (oldMode & 0x7F) | 0x10

	p.i2cBus.WriteReg(MODE1, []byte{newMode & WORD})
	p.i2cBus.WriteReg(PRESCALE, []byte{byte(prescale) & WORD})
	p.i2cBus.WriteReg(MODE1, []byte{oldMode & WORD})

	time.Sleep(5 * time.Millisecond)

	p.i2cBus.WriteReg(MODE1, []byte{oldMode&WORD | 0x80})
}

func (p *PCA9685) setAllPwm(on uint16, off uint16) {
	onB := byte(on)
	offB := byte(off)

	p.i2cBus.WriteReg(ALL_LED_ON_L, []byte{onB & WORD})
	p.i2cBus.WriteReg(ALL_LED_ON_H, []byte{onB & WORD})
	p.i2cBus.WriteReg(ALL_LED_OFF_L, []byte{offB & WORD})
	p.i2cBus.WriteReg(ALL_LED_OFF_H, []byte{offB & WORD})
}

func (p *PCA9685) setPwm(pwm int, on uint16, off uint16) {

	p.i2cBus.WriteReg(LED0_ON_H+byte(4)*byte(pwm), []byte{byte(on >> 8)})
	p.i2cBus.WriteReg(LED0_ON_L+byte(4)*byte(pwm), []byte{byte(on & 0x00FF)})
	p.i2cBus.WriteReg(LED0_OFF_H+byte(4)*byte(pwm), []byte{byte(off >> 8)})
	p.i2cBus.WriteReg(LED0_OFF_L+byte(4)*byte(pwm), []byte{byte(off & 0x00FF)})
}

func (pwm *Pwm) setPercentage(percentage float32) error {
	if percentage < 0.0 || percentage > 100.0 {
		return errors.New(fmt.Sprintf("Percentage must be between 0.0 and 100.0. Got %v.", percentage))
	}

	pwm.pca.setPwm(pwm.pin, 0, uint16((percentage/100)*float32(pwm.pca.maxPulse)))
	pwm.pca.log.Info(fmt.Sprintf("Setting pwm #%v to %v%% (%v) at \"%v\" device.", pwm.pin, percentage, uint16((percentage/100)*float32(pwm.pca.maxPulse)), pwm.pca.name))

	return nil
}