package main

import (
	"fmt"
	"log"
	"net"
	"os"
	"os/signal"
	"strconv"
	"sync"
	"time"

	pb "dronectrl/controlpb"

	"github.com/golang/protobuf/proto"
	"github.com/go-daq/smbus"
	"periph.io/x/conn/v3/gpio"
	"periph.io/x/host/v3"
	"periph.io/x/host/v3/bcm283x"
	"periph.io/x/host/v3/rpi"
)

const (
	udpPort = ":3344"
)

type Command struct {
	timestamp int64
	servo1    float32 // 0..1
	servo2    float32
	brushless float32
}

func scaleDuty(f float32) gpio.Duty {
	if f < 0 {
		f = 0
	}
	if f > 1 {
		f = 1
	}
	return gpio.Duty(float32(gpio.DutyMax) * f)
}

func pollData() {
	ticker := time.NewTicker(10 * time.Millisecond)
	defer ticker.Stop()
	for {
		select {
		case <-ticker.C:
		}
	}
}

func recvLoop(cmdCh chan Command) {
	var latestTimestamp int64
	var mu sync.Mutex

	bus, err := smbus.Open(0, 0x69)
	if err != nil {
		t.Fatalf("open error: %v\n", err)
	}
	defer c.Close()

	v, err := c.ReadReg(0x69, 0x1)
	if err != nil {
		t.Fatalf("read-reg error: %v\n", err)
	}
	t.Logf("v=%v\n", v)


	addr, err := net.ResolveUDPAddr("udp", udpPort)
	if err != nil {
		log.Fatalf("ResolveUDPAddr error: %v", err)
	}

	conn, err := net.ListenUDP("udp", addr)
	if err != nil {
		log.Fatalf("ListenUDP error: %v", err)
	}
	defer conn.Close()

	buf := make([]byte, 1024)

	for {
		n, _, err := conn.ReadFromUDP(buf)
		if err != nil {
			log.Printf("UDP read error: %v", err)
			continue
		}

		var msg pb.ServoCommand
		if err := proto.Unmarshal(buf[:n], &msg); err != nil {
			log.Printf("proto unmarshal error: %v", err)
			continue
		}

		mu.Lock()
		if msg.Timestamp > latestTimestamp {
			latestTimestamp = msg.Timestamp
			mu.Unlock()

			cmdCh <- Command{
				timestamp: msg.Timestamp,
				servo1:    msg.Servo1,
				servo2:    msg.Servo2,
				brushless: msg.Brushless,
			}
		} else {
			mu.Unlock()
		}
	}
}

func ctrlLoop(cmdCh chan Command) {
	const (
		servoPeriod     = 20 * time.Millisecond
		brushlessPeriod = 5 * time.Millisecond
	)

	// servoFreq := physic.PeriodToFrequency(20 * time.Millisecond)
	// brushlessFreq := physic.PeriodToFrequency(5 * time.Millisecond)

	for cmd := range cmdCh {
		fmt.Println("cmd:", cmd)
		fmt.Println("cmd(freq):", scaleDuty(cmd.brushless))

		duty := int((cmd.servo1 * 1000000) + 1000000)
		str := strconv.Itoa(duty / 1000 * 1000) // Reduce precision - the servos support only us
		fmt.Println("servo1 (duty):", str)
		if err := os.WriteFile("/sys/class/pwm/pwmchip0/pwm3/duty_cycle", []byte(str), 0666); err != nil {
			log.Printf("PWM brushless error: %v", err)
		}

		duty = int(2000000 - (cmd.servo2 * 1000000))
		str = strconv.Itoa(duty / 1000 * 1000) // Reduce precision -  the servos support only us
		fmt.Println("servo2 (duty):", str)
		if err := os.WriteFile("/sys/class/pwm/pwmchip0/pwm1/duty_cycle", []byte(str), 0666); err != nil {
			log.Printf("PWM brushless error: %v", err)
		}

		duty = int((cmd.brushless * 936000) + 1160000)
		str = strconv.Itoa(duty / 1000 * 1000) // Reduce precision - the ASC does not support more
		fmt.Println("brushless (duty):", str)
		if err := os.WriteFile("/sys/class/pwm/pwmchip0/pwm0/duty_cycle", []byte(str), 0666); err != nil {
			log.Printf("PWM brushless error: %v", err)
		}
	}
}

func main() {
	if _, err := host.Init(); err != nil {
		log.Fatalf("periph host.Init failed: %v", err)
	}

	pins := []gpio.PinOut{bcm283x.GPIO12, bcm283x.GPIO13, rpi.P1_12}
	for _, p := range pins {
		defer p.Halt()
	}

	cmdCh := make(chan Command, 10)

	go recvLoop(cmdCh)
	go ctrlLoop(cmdCh)
	go pollData(cmdCh)

	sigCh := make(chan os.Signal, 1)
	signal.Notify(sigCh, os.Interrupt)
	<-sigCh

	log.Println("Exiting")
}
