package main

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

	"github.com/golang/protobuf/proto"
	"github.com/veandco/go-sdl2/sdl"

	pb "dronectrl/controlpb"
)

const (
	udpAddr = "10.5.0.2:3344"
)

func normalizeInput(axisVal int) float32 {
	normalized := float32((-int(axisVal) + 32767)) / 65534.0
	if normalized < 0 {
		return 0
	} else if normalized > 1 {
		return 1
	}
	return normalized
}

func mapRange(value, fromMin, fromMax, toMin, toMax float32) float32 {
	if fromMax == fromMin {
		panic("mapping error")
	}
	
	return (value - fromMin) * (toMax - toMin) / (fromMax - fromMin) + toMin
}

func getServos(lr, td float32) (srv1 float32, srv2 float32)  {
	srv1LR := lr
	srv2LR := 1-lr 

	srv1TD := 1-td
	srv2TD := 1-td

	srv1 = (srv1TD+(srv1TD)+srv1LR)/3 * 1.5
	srv2 = (srv2TD+(srv2TD)+srv2LR)/3 * 1.5

	srv1 = mapRange(srv1, 0.25, 1.5, -0.2, 1.5)
	srv2 = mapRange(srv2, 0.25, 1.5, -0.2, 1.5)

	// if srv1 < 0 {
	// 	srv1 = 0
	// } else if srv1 > 1 {
	// 	srv1 = 1
	// }
	//
	// if srv2 < 0 {
	// 	srv2 = 0
	// } else if srv2 > 1 {
	// 	srv2 = 1
	// }

	return
}

func main() {
	if err := sdl.Init(sdl.INIT_JOYSTICK); err != nil {
		log.Fatalf("Failed to init SDL: %v", err)
	}
	defer sdl.Quit()

	if sdl.NumJoysticks() < 1 {
		log.Fatalf("No joysticks found")
	}

	joy := sdl.JoystickOpen(0)
	if joy == nil {
		log.Fatalf("Failed to open joystick: %v", sdl.GetError())
	}
	defer joy.Close()

	conn, err := net.Dial("udp", udpAddr)
	if err != nil {
		log.Fatalf("Failed to dial UDP: %v", err)
	}
	defer conn.Close()

	log.Println("Joystick opened, starting control loop")

	ticker := time.NewTicker(50 * time.Millisecond)
	defer ticker.Stop()

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

	for {
		select {
		case <-sigCh:
			log.Println("Exiting on interrupt")
			return
		case <-ticker.C:
			sdl.PumpEvents()

			axisThrottle := int(joy.Axis(3))
			normalizedThrottle := normalizeInput(axisThrottle)

			// LIMIT!!!
			if normalizedThrottle > 0.68 {
				normalizedThrottle = 0.68
			}

			axisTopDown := int(joy.Axis(1))
			normalizedTopDown := normalizeInput(axisTopDown)

			axisLeftRight := int(joy.Axis(0))
			normalizedLeftRight := normalizeInput(axisLeftRight)

			srv1, srv2 := getServos(normalizedLeftRight, normalizedTopDown)

			msg := &pb.ServoCommand{
				Timestamp: time.Now().UnixNano(),
				Brushless: normalizedThrottle,
				Servo1:    srv1,
				Servo2:    srv2,
			}

			fmt.Println("msg:", msg)

			data, err := proto.Marshal(msg)
			if err != nil {
				log.Printf("Failed to marshal protobuf: %v", err)
				continue
			}

			_, err = conn.Write(data)
			if err != nil {
				log.Printf("Failed to send UDP data: %v", err)
			}
		}
	}
}
