package main

import (
	"fmt"
	"github.com/blackjack/webcam"
	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 handleWebcam(client mqtt.Client) {
	cam, err := webcam.Open("/dev/video0")
	if err != nil {
		log.Fatalf("Could not open webcam: %v\n", err)
	}
	defer cam.Close()

	supportedFormat := cam.GetSupportedFormats()
	//log.Printf("Supported formats:\n")
	imageFormat := webcam.PixelFormat(0)
	for format, description := range supportedFormat {
		if description == "10-bit Bayer GBGB/RGRG" {
			imageFormat = format
		}
	}

	//width, height := 1296, 972
	//width, height := uint32(2592), uint32(1944)
	width, height := uint32(640), uint32(480)

	if imageFormat != webcam.PixelFormat(0) {
		_, _, _, err = cam.SetImageFormat(imageFormat, width, height)
		if err != nil {
			log.Fatalf("Could not set image format: %v\n", err)
		}
	}

	supportedFramerates := cam.GetSupportedFramerates(imageFormat, width, height)
	log.Printf("Supported frame rates:\n")
	for _, format := range supportedFramerates {
		log.Printf(" - %s\n", format)
	}

	controls := cam.GetControls()
	log.Printf("Controls:\n")
	for _, control := range controls {
		log.Printf(" - %s\n", control)
	}

	//cam.SetAutoWhiteBalance(true)
	//cam.SetControl()

	//err = cam.SetFramerate(10)
	//if err != nil {
	//	log.Fatalf("Could not set framerate: %v\n", err)
	//}

	err = cam.StartStreaming()
	if err != nil {
		log.Fatalf("Could not start streaming webcam: %v\n", err)
	}

	count := 0
	for {
		err = cam.WaitForFrame(100)
		if err != nil {
			log.Fatalf("Could not start streaming webcam: %v\n", err)
		}
		frame, err := cam.ReadFrame()
		if err != nil {
			log.Fatalf("Could not read frame: %v\n", err)
		}
		if len(frame) > 0 {
			count++
			if count > 30*4 {
				print("Publishing frame\n")
				publishTelemetry(client, "camfeed", frame)
				return
			}
		}
	}
}

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()
	//go handleWebcam(client)

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

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