package main

import (
	"encoding/json"
	"fmt"
	mqtt "github.com/eclipse/paho.mqtt.golang"
	"log"
	"time"
)

const broker = "tcp://mqtt.seeseepuff.be:1883"
const telemetryTopic = "spider/telemetry/"

func publishTelemetry(client mqtt.Client, topic string, payload any) {
	token := client.Publish(telemetryTopic+topic, 0, false, payload)
	token.Wait()
	if token.Error() != nil {
		log.Printf("Error publishing servo battery state: %v\n", token.Error())
	}
}

func publishServoBatteryTelemetry(client mqtt.Client) {
	value, err := GetBatteryServo()
	if err != nil {
		return
	}
	publishTelemetry(client, "servo_battery", fmt.Sprintf("%f", value))
}

func publishCpuBatteryTelemetry(client mqtt.Client) {
	value, err := GetBatteryCPU()
	if err != nil {
		return
	}
	publishTelemetry(client, "cpu_battery", fmt.Sprintf("%f", value))
}

func publishSlowTelemetry(client mqtt.Client) {
	go publishServoBatteryTelemetry(client)
	go publishCpuBatteryTelemetry(client)
}

func onPing(client mqtt.Client, msg mqtt.Message) {
	log.Print("Got ping")
	publishTelemetry(client, "pong", msg.Payload())
}

func onSetCameraXY(client mqtt.Client, msg mqtt.Message) {
	log.Print("Got move camera")
	payload := make(map[string]float64)
	err := json.Unmarshal(msg.Payload(), &payload)
	if err != nil {
		log.Printf("Error unmarshalling set_camera_xy payload: %v\n", err)
		return
	}
	x, ok := payload["x"]
	if !ok {
		log.Printf("Missing x in set_camera_xy")
		return
	}
	y, ok := payload["y"]
	if !ok {
		log.Printf("Missing y in set_camera_xy")
		return
	}
	SetServoAngle(ServoHeadHorizontal, x)
	SetServoAngle(ServoHeadVertical, y)
}

func subscribe(client mqtt.Client, topic string, handler mqtt.MessageHandler) {
	token := client.Subscribe(topic, 0, handler)
	token.Wait()
	if token.Error() != nil {
		log.Fatalf("Failed to subscribe to command topic: %v\n", token.Error())
	}
}

func main() {
	opts := mqtt.NewClientOptions()
	opts.AddBroker(broker)
	opts.SetClientID("spider-host-client")
	opts.SetResumeSubs(true)
	client := mqtt.NewClient(opts)

	token := client.Connect()
	token.Wait()
	if token.Error() != nil {
		log.Fatalf("Error connecting to MQTT broker: %v\n", token.Error())
	}

	log.Print("Subscribing to command topics")
	subscribe(client, "spider/command/ping", onPing)
	subscribe(client, "spider/command/set_camera_xy", onSetCameraXY)

	InitBattery()
	InitServo()

	ServosOff()

	slowTelemetry := time.NewTicker(3 * time.Second)
	defer slowTelemetry.Stop()

	//moveServo := time.NewTicker(100 * time.Millisecond)
	//defer moveServo.Stop()

	publishSlowTelemetry(client)
	for {
		select {
		case <-slowTelemetry.C:
			publishSlowTelemetry(client)
			//case <-moveServo.C:
			//	seconds := time.Now().UnixMilli()
			//	angle := (math.Cos(float64(seconds)/2_500) + 1) * 0.5
			//	log.Printf("Target angle: %.1f\n", angle)
			//	SetServoAngle(ServoHeadHorizontal, angle)
		}
	}
}