package main

import (
	"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 main() {
	opts := mqtt.NewClientOptions()
	opts.AddBroker(broker)
	opts.SetClientID("spider-host-client")
	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")
	token = client.Subscribe("spider/command/ping", 0, onPing)
	token.Wait()
	if token.Error() != nil {
		log.Fatalf("Failed to subscribe to command topic: %v\n", token.Error())
	}

	InitBattery()

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

	publishSlowTelemetry(client)
	for {
		select {
		case <-slowTelemetry.C:
			publishSlowTelemetry(client)
		}
	}
}