package main

import (
	"encoding/binary"
	"fmt"
	"machine"
	"math/rand"
	"time"

	"tinygo.org/x/bluetooth"
	"tinygo.org/x/drivers/lsm6ds3tr"
)

// TinyGo Drivers Docs:
// https://github.com/tinygo-org/drivers

// TinyGo Bluetooth Docs:
// https://github.com/tinygo-org/bluetooth

var adapter = bluetooth.DefaultAdapter

var ledColor = [3]byte{0x00, 0x00, 0xff}
var leds = [3]machine.Pin{machine.LED_RED, machine.LED_GREEN, machine.LED_BLUE}
var hasColorChange = true

var senseAccelerationData string = "-0.000 -0.000 -0.000"

var senseRotationData string = "-0.000 -0.000 -0.000"

var bleConnected bool = false

var (
	LSM6DS3TRService = [16]byte{0x4C, 0x53, 0x4D, 0x36, 0x44, 0x53, 0x33, 0x54, 0x52, 0x53, 0x65, 0x72, 0x76, 0x69, 0x63, 0x65}
	accelerationData = [16]byte{0x61, 0x63, 0x63, 0x65, 0x6C, 0x65, 0x72, 0x61, 0x74, 0x69, 0x6F, 0x6E, 0x44, 0x61, 0x74, 0x61}
	tempSenseService = [16]byte{0x74, 0x65, 0x6D, 0x70, 0x53, 0x65, 0x6E, 0x73, 0x65, 0x53, 0x65, 0x72, 0x76, 0x69, 0x63, 0x65}
	temperatureSense = [16]byte{0x74, 0x65, 0x6D, 0x70, 0x65, 0x72, 0x61, 0x74, 0x75, 0x72, 0x65, 0x53, 0x65, 0x6E, 0x73, 0x65}
)

func main() {
	// Configure LSM6DS3TR
	machine.I2C0.Configure(machine.I2CConfig{})

	accel := lsm6ds3tr.New(machine.I2C0)
	err := accel.Configure(lsm6ds3tr.Configuration{})
	if err != nil {
		for {
			println("Failed to configure", err.Error())
			time.Sleep(time.Second)
		}
	}
	//
	// for _, led := range leds {
	// 	led.Configure(machine.PinConfig{Mode: machine.PinOutput})
	// }

	// Configure Bluetooth
	must("enable BLE stack", adapter.Enable())
	adv := adapter.DefaultAdvertisement()
	must("config adv", adv.Configure(bluetooth.AdvertisementOptions{
		LocalName: "Go Bluetooth",
		ManufacturerData: []bluetooth.ManufacturerDataElement{
			// 0xFFFF: Special Use/Default ID
			// Bluetooth Company Identifiers:
			// https://gist.github.com/angorb/f92f76108b98bb0d81c74f60671e9c67
			{CompanyID: 0xffff, Data: []byte{0x01, 0x02}},
		},
	}))

	adapter.SetConnectHandler(func(device bluetooth.Device, connected bool) {
		if connected {
			println("connected, not advertising...")
			// leds[0].Low()
			// leds[1].High()
			// ledColor[0] = 0
			// ledColor[1] = 1
			// ledColor[2] = 0
			// hasColorChange = true
			bleConnected = true
		} else {
			println("disconnected, advertising...")
			// leds[0].High()
			// leds[1].Low()
			// ledColor[0] = 0
			// ledColor[1] = 0
			// ledColor[2] = 1
			// hasColorChange = true
			bleConnected = false
		}
	})
	//
	// Start Bluetooth advertisment
	must("start adv", adv.Start())

	var senseCharacteristic bluetooth.Characteristic
	must("add sense service", adapter.AddService(&bluetooth.Service{
		UUID: bluetooth.NewUUID(LSM6DS3TRService),
		Characteristics: []bluetooth.CharacteristicConfig{
			{
				Handle: &senseCharacteristic,
				UUID:   bluetooth.NewUUID(accelerationData),
				Value:  []byte(senseAccelerationData),
				Flags:  bluetooth.CharacteristicNotifyPermission | bluetooth.CharacteristicReadPermission | bluetooth.CharacteristicWritePermission | bluetooth.CharacteristicWriteWithoutResponsePermission,
			},
		},
	}))

	// var tempCharacteristic bluetooth.Characteristic
	// must("add temperature service", adapter.AddService(&bluetooth.Service{
	// 	UUID: bluetooth.NewUUID(tempSenseService),
	// 	Characteristics: []bluetooth.CharacteristicConfig{
	// 		{
	// 			Handle: &tempCharacteristic,
	// 			UUID:   bluetooth.NewUUID(temperatureSense),
	// 			Value:  []byte(tempData),
	// 			Flags:  bluetooth.CharacteristicNotifyPermission | bluetooth.CharacteristicReadPermission,
	// 		},
	// 	},
	// }))

	// Main Loop
	for {
		time.Sleep(100 * time.Millisecond)

		// for !hasColorChange {
		// 	for i, led := range leds {
		// 		led.Set(ledColor[i] == 0)
		// 	}
		// 	time.Sleep(10 * time.Millisecond)
		// }
		// hasColorChange = false

		// Only read and update sensor data
		// with an active bluetooth connection
		if bleConnected {
			X, Y, Z, _ := accel.ReadRotation()
			rotX := fmt.Sprintf("rX=%f", float32(X)/100000000)
			rotY := fmt.Sprintf("rY=%f", float32(Y)/100000000)
			rotZ := fmt.Sprintf("rZ=%f", float32(Z)/100000000)

			x, y, z, _ := accel.ReadAcceleration()
			accel := fmt.Sprintf("%.3f,%.3f,%.3f", float32(x)/1000000, float32(y)/1000000, float32(z)/1000000)
			//senseAccelerationData = dataTest

			//tempTest, _ := accel.ReadTemperature()
			//tempTestBytes := []byte{}
			//tempData = fmt.Sprintf("%i", tempTest)

			//senseCharacteristic.Write([]byte(dataRotation))
			//senseCharacteristic.Write([]byte("-0.0,-1.2,-5.6"))
			senseCharacteristic.Write([]byte(rotX))
			senseCharacteristic.Write([]byte(rotY))
			senseCharacteristic.Write([]byte(rotZ))
			senseCharacteristic.Write([]byte(accel))

			// bs := make([]byte, 4)
			// binary.LittleEndian.PutUint32(bs, X)

			//binary.LittleEndian.AppendUint32(tempTestBytes, uint32(tempTest))
			//tempCharacteristic.Write(tempTestBytes)
		}
	}
}

func must(action string, err error) {
	if err != nil {
		panic("failed to " + action + ": " + err.Error())
	}
}

// Returns an int >= min, < max
func randomInt(min, max int) uint8 {
	return uint8(min + rand.Intn(max-min))
}