package main

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

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

var adapter = bluetooth.DefaultAdapter

var isBleConnected bool = false

var isCapturing 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}
	unixTimeStampRst = [16]byte{0x75, 0x6E, 0x69, 0x78, 0x54, 0x69, 0x6D, 0x65, 0x53, 0x74, 0x61, 0x6D, 0x70, 0x52, 0x73, 0x74}
	capturingService = [16]byte{0x63, 0x61, 0x70, 0x74, 0x75, 0x72, 0x69, 0x6E, 0x67, 0x53, 0x65, 0x72, 0x76, 0x69, 0x63, 0x65}
	//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}
)

var currentTimeStamp time.Time = time.Now()
var lastTimeStamp time.Time = time.Now()

const sleepDuration time.Duration = time.Millisecond * 100

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)
		}
	}

	// 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 {
			isBleConnected = true
			adv.Stop()
		} else {
			isBleConnected = false
			isCapturing = false
			adv.Start()
		}
	})
	//
	// 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),
				// can only send a max amount of 20 bytes in one packet
				//Value: []byte{},
				Flags: bluetooth.CharacteristicNotifyPermission | bluetooth.CharacteristicReadPermission | bluetooth.CharacteristicWritePermission | bluetooth.CharacteristicWriteWithoutResponsePermission,
			},
			{
				UUID:  bluetooth.NewUUID(unixTimeStampRst),
				Flags: bluetooth.CharacteristicNotifyPermission | bluetooth.CharacteristicReadPermission | bluetooth.CharacteristicWritePermission | bluetooth.CharacteristicWriteWithoutResponsePermission,
				WriteEvent: func(client bluetooth.Connection, offset int, value []byte) {
					if len(value) != 8 {
						return
					}
					millisFromEpoch := binary.BigEndian.Uint64(value)
					currentTimeStamp = time.Unix(0, int64(millisFromEpoch)*int64(time.Millisecond))
				},
			},
			{
				UUID:  bluetooth.NewUUID(capturingService),
				Flags: bluetooth.CharacteristicNotifyPermission | bluetooth.CharacteristicReadPermission | bluetooth.CharacteristicWritePermission | bluetooth.CharacteristicWriteWithoutResponsePermission,
				WriteEvent: func(client bluetooth.Connection, offset int, value []byte) {
					if len(value) != 1 {
						return
					}
					if value[0] == 1 {
						isCapturing = true
					} else {
						isCapturing = false
					}
				},
			},
		},
	}))

	// 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 {
		// Only read and update sensor data
		// with an active bluetooth connection
		// and when `isCapturing` is set to true
		if isBleConnected && isCapturing {
			X, Y, Z, _ := accel.ReadRotation()
			x, y, z, _ := accel.ReadAcceleration()

			arrRot := valuesToByteArray(X, Y, Z, int8(1))
			arrAcc := valuesToByteArray(x, y, z, int8(2))

			currentTimeStamp = currentTimeStamp.Add(time.Now().Sub(lastTimeStamp))
			fmt.Println("TIME: ", time.Now().Sub(lastTimeStamp))
			arrTime := timeStampToByteArray(currentTimeStamp.UnixMilli(), int8(3))

			senseCharacteristic.Write(arrRot)
			senseCharacteristic.Write(arrAcc)
			senseCharacteristic.Write(arrTime)
		}

		if isCapturing {
			lastTimeStamp = time.Now()
		}
		time.Sleep(sleepDuration)
	}
}

func valuesToByteArray(x int32, y int32, z int32, p int8) []byte {
	byteSlice := make([]byte, 13)
	binary.LittleEndian.PutUint32(byteSlice, uint32(x))
	binary.LittleEndian.PutUint32(byteSlice[4:], uint32(y))
	binary.LittleEndian.PutUint32(byteSlice[8:], uint32(z))
	byteSlice[12] = byte(p)
	return byteSlice
}

func timeStampToByteArray(value int64, p int8) []byte {
	byteSlice := make([]byte, 9)
	binary.LittleEndian.PutUint64(byteSlice, uint64(value))
	byteSlice[8] = byte(p)
	return byteSlice
}

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