package main

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

var ads *ADS7830

//var mpu *i2c.MPU6050Driver
//mpu = i2c.NewMPU6050Driver(rpi, i2c.WithBus(0), i2c.WithAddress(0x40))

func InitBattery() {
	rpi := GetAdaptor()
	rpi.Connect()
	ads = NewADS7830(rpi, i2c.WithBus(1), i2c.WithAddress(0x48))
	ads.Start()
}

func GetBatteryServo() (float32, error) {
	value, err := getBattery(0)
	if err != nil {
		log.Print("Error reading battery: ", err)
	}
	log.Print("Servo Battery state: ", value)
	return value, err
}

func GetBatteryCPU() (float32, error) {
	value, err := getBattery(4)
	if err != nil {
		log.Print("Error reading battery: ", err)
	}
	log.Print("CPU Battery state: ", value)
	return value, err
}

func getBattery(channel int) (float32, error) {
	value, err := ads.AnalogRead(channel)
	if err != nil {
		return 0, err
	}
	converted := float32(value) / 255 * 5 * 3
	return converted, nil
}