package main

import (
	"gobot.io/x/gobot/drivers/i2c"
	"log"
	"math"
)

type Servo int

const (
	ServoHeadHorizontal Servo = iota
	ServoHeadVertical
	ServoCount
)

type ServoChannel struct {
	controller *i2c.PCA9685Driver
	channel    int
}

var servoController1 *i2c.PCA9685Driver
var servoController2 *i2c.PCA9685Driver

// var servos = new([ServoCount]*gpio.ServoDriver)
var servos = new([ServoCount]*ServoChannel)

func InitServo() {
	adaptor := GetAdaptor()
	servoController1 = i2c.NewPCA9685Driver(adaptor, i2c.WithBus(1), i2c.WithAddress(0x40))
	servoController2 = i2c.NewPCA9685Driver(adaptor, i2c.WithBus(1), i2c.WithAddress(0x41))

	createServo(ServoHeadHorizontal, servoController2, 1)
	createServo(ServoHeadVertical, servoController2, 0)

	err := servoController1.Start()
	if err != nil {
		log.Print("Could not start servo controller 1: ", err)
	}
	err = servoController2.Start()
	if err != nil {
		log.Print("Could not start servo controller 1: ", err)
	}

	err = servoController1.SetPWMFreq(50)
	if err != nil {
		log.Print("Could not set servo controller 1 frequency: ", err)
	}
	err = servoController2.SetPWMFreq(50)
	if err != nil {
		log.Print("Could not set servo controller 2 frequency: ", err)
	}
	// Halt the controllers to stop any current movement
	ServosOff()
	log.Print("Started servos")
}

func ServosOff() {
	err := servoController1.Halt()
	if err != nil {
		log.Print("Could not stop servo controller 1: ", err)
	}
	err = servoController2.Halt()
	if err != nil {
		log.Print("Could not stop servo controller 2: ", err)
	}
}

// SetServoAngle Sets the angle to a value of 0 - 1
func SetServoAngle(servo Servo, angle float64) {
	pulseDuration := angle + 1
	pulseDurationRatio := pulseDuration / 20
	pulseDurationTicks := pulseDurationRatio * 4095
	servoDriver := servos[servo]
	err := servoDriver.controller.SetPWM(servoDriver.channel, 0, uint16(math.Floor(pulseDurationTicks)))
	if err != nil {
		log.Printf("Could not set servo %d to angle %0.1f: %v\n", servo, angle, err)
	}
}

func createServo(servo Servo, pwmDriver *i2c.PCA9685Driver, channel int) {
	//servoDriver := gpio.NewServoDriver(pwmDriver, channel)
	//servos[servo] = servoDriver
	servos[servo] = &ServoChannel{
		controller: pwmDriver,
		channel:    channel,
	}
}