// Connects to an LSM6DSOX I2C a 6 axis Inertial Measurement Unit (IMU)
package main

import (
	"fmt"
	"machine"
	"time"

	"tinygo.org/x/drivers/lsm6dsox"
)

const (
	PLOTTER           = true
	SHOW_ACCELERATION = false
	SHOW_ROTATION     = true
	SHOW_TEMPERATURE  = false

	// calibration is naive, good enough for a demo: do not shake a second after flashing
	GYRO_CALIBRATION = true
)

var cal = [...]float32{0, 0, 0}

func main() {

	machine.I2C0.Configure(machine.I2CConfig{})

	device := lsm6dsox.New(machine.I2C0)
	err := device.Configure(lsm6dsox.Configuration{
		AccelRange:      lsm6dsox.ACCEL_2G,
		AccelSampleRate: lsm6dsox.ACCEL_SR_104,
		GyroRange:       lsm6dsox.GYRO_250DPS,
		GyroSampleRate:  lsm6dsox.GYRO_SR_104,
	})
	if err != nil {
		for {
			println("Failed to configure", err.Error())
			time.Sleep(time.Second)
		}
	}

	for {

		if !device.Connected() {
			println("LSM6DSOX not connected")
			time.Sleep(time.Second)
			continue
		}

		// heuristic: after successful calibration the value can't be 0
		if GYRO_CALIBRATION && cal[0] == 0 {
			calibrateGyro(device)
		}

		ax, ay, az, _ := device.ReadAcceleration()
		gx, gy, gz, _ := device.ReadRotation()
		t, _ := device.ReadTemperature()

		if PLOTTER {
			printPlotter(ax, ay, az, gx, gy, gz, t)
			time.Sleep(time.Millisecond * 100)
		} else {
			printMonitor(ax, ay, az, gx, gy, gz, t)
			time.Sleep(time.Millisecond * 1000)
		}

	}

}

func calibrateGyro(device *lsm6dsox.Device) {
	for i := 0; i < 100; i++ {
		gx, gy, gz, _ := device.ReadRotation()
		cal[0] += float32(gx) / 1000000
		cal[1] += float32(gy) / 1000000
		cal[2] += float32(gz) / 1000000
		time.Sleep(time.Millisecond * 10)
	}
	cal[0] /= 100
	cal[1] /= 100
	cal[2] /= 100
}

// Arduino IDE's Serial Plotter
func printPlotter(ax, ay, az, gx, gy, gz, t int32) {
	if SHOW_ACCELERATION {
		fmt.Printf("AX:%f, AY:%f, AZ:%f,", axis(ax, 0), axis(ay, 0), axis(az, 0))
	}
	if SHOW_ROTATION {
		fmt.Printf("GX:%f, GY:%f, GZ:%f,", axis(gx, cal[0]), axis(gy, cal[1]), axis(gz, cal[2]))
	}
	if SHOW_TEMPERATURE {
		fmt.Printf("T:%f", float32(t)/1000)
	}
	println()
}

// Any Serial Monitor
func printMonitor(ax, ay, az, gx, gy, gz, t int32) {
	if SHOW_ACCELERATION {
		fmt.Printf("Acceleration: %f, %f, %f\r\n", axis(ax, 0), axis(ay, 0), axis(az, 0))
	}
	if SHOW_ROTATION {
		fmt.Printf("Rotation: %f, %f, %f\r\n", axis(gx, cal[0]), axis(gy, cal[1]), axis(gz, cal[2]))
	}
	if SHOW_TEMPERATURE {
		fmt.Printf("Temperature C: %f\r\n", float32(t)/1000)
	}
	println()
}

func axis(raw int32, cal float32) float32 {
	return float32(raw)/1000000 - cal
}