package main

import (
	"fmt"
	"math"
	"time"

	"dronectrl/filter"
	"dronectrl/mpu6050"
)

func main() {
	mpu, err := mpu6050.New(1, 0x68)
	if err != nil {
		panic(err)
	}
	defer mpu.Close()

	fltr := filter.MadgwickFilter{
		Quaternion: filter.Quaternion{1, 0, 0, 0},
		Beta:       0.05,
	}

	sampleRate := 800.0 // Hz
	dt := 1.0 / sampleRate
	ticker := time.NewTicker(time.Duration(dt*1000) * time.Millisecond)

	for range ticker.C {
		// gx, gy, gz := 0.0, 0.0, math.Pi/4
		// ax, ay, az := 0.0, 0.0, -1.0

		t, _ := mpu.GetTemp()
		ax, ay, az, _ := mpu.GetAccel(false) // rad/s
		gxDps, gyDps, gzDps, _ := mpu.GetGyro() // m/s²
		gx, gy, gz := gxDps * (math.Pi / 180), gyDps * (math.Pi / 180), gzDps * (math.Pi / 180)

		fmt.Printf("Temp : %6.2f °C\n", t)
		fmt.Printf("Accel: X=%8.3f  Y=%8.3f  Z=%8.3f\n", ax, ay, az)
		fmt.Printf("Gyro : X=%8.3f  Y=%8.3f  Z=%8.3f\n", gx, gy, gz)

		fltr.Update(gx, gy, gz, ax, ay, az, dt)

		roll, pitch, yaw := fltr.Quaternion.ToEuler()
		fmt.Printf("Roll: %6.2f°, Pitch: %6.2f°, Yaw: %6.2f°\n",
			roll*180/math.Pi, pitch*180/math.Pi, yaw*180/math.Pi)
	}
}
