package main

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

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

var adapter = bluetooth.DefaultAdapter

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

var heartRate uint8 = 75 // 75bpm

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

var (
	// a0b40001-926d-4d61-98df-8c5c62ee53b3
	serviceUUID      = [16]byte{0xa0, 0xb4, 0x00, 0x01, 0x92, 0x6d, 0x4d, 0x61, 0x98, 0xdf, 0x8c, 0x5c, 0x62, 0xee, 0x53, 0xb3}
	senseServiceUUID = [16]byte{0xa0, 0xb4, 0x00, 0x03, 0x92, 0x6d, 0x4d, 0x61, 0x98, 0xdf, 0x8c, 0x5c, 0x62, 0xee, 0x53, 0xb3}
	charUUID         = [16]byte{0xa0, 0xb4, 0x00, 0x02, 0x92, 0x6d, 0x4d, 0x61, 0x98, 0xdf, 0x8c, 0x5c, 0x62, 0xee, 0x53, 0xb3}
	senseCharUUID    = [16]byte{0xa0, 0xb4, 0x00, 0x04, 0x92, 0x6d, 0x4d, 0x61, 0x98, 0xdf, 0x8c, 0x5c, 0x62, 0xee, 0x53, 0xb3}
)

func main() {
	println("starting")

	// 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{
			{CompanyID: 0xffff, Data: []byte{0x01, 0x02}},
		},
	}))

	adapter.SetConnectHandler(func(device bluetooth.Device, connected bool) {
		if connected {
			println("connected, not advertising...")
		} else {
			println("disconnected, advertising...")
		}
	})

	must("start adv", adv.Start())

	println("advertising...")
	// address, _ := adapter.Address()

	var ledColorCharacteristic bluetooth.Characteristic
	must("add led service", adapter.AddService(&bluetooth.Service{
		UUID: bluetooth.NewUUID(serviceUUID),
		Characteristics: []bluetooth.CharacteristicConfig{
			{
				Handle: &ledColorCharacteristic,
				UUID:   bluetooth.NewUUID(charUUID),
				Value:  ledColor[:],
				Flags:  bluetooth.CharacteristicReadPermission | bluetooth.CharacteristicWritePermission | bluetooth.CharacteristicWriteWithoutResponsePermission,
				WriteEvent: func(client bluetooth.Connection, offset int, value []byte) {
					if offset != 0 || len(value) != 3 {
						return
					}
					ledColor[0] = value[0]
					ledColor[1] = value[1]
					ledColor[2] = value[2]
					hasColorChange = true
				},
			},
		},
	}))

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

	for _, led := range leds {
		led.Configure(machine.PinConfig{Mode: machine.PinOutput})
	}

	// Loop
	//nextBeat := time.Now()
	for {
		time.Sleep(500 * time.Millisecond)
		//nextBeat = nextBeat.Add(time.Minute / time.Duration(heartRate))
		//println("tick", time.Now().Format("04:05.000"))
		//time.Sleep(nextBeat.Sub(time.Now()))
		// for !hasColorChange {
		// 	time.Sleep(10 * time.Millisecond)
		// }
		// hasColorChange = false
		// for i, led := range leds {
		// 	led.Set(ledColor[i] == 0)
		// }

		// random variation in heartrate
		// heartRate = randomInt(65, 85)
		//heartRate = 85

		// and push the next notification

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

		senseCharacteristic.Write([]byte(senseAccelerationData))

		println(senseAccelerationData)

		//x, _, _, _ := accel.ReadAcceleration()

		//heartRate = uint8(x)

		//dataTest := fmt.Sprintf("%d, %d, %d", float32(x)/1000000, float32(y)/1000000, float32(z)/1000000)
		//dataSense = dataTest
		// dataTest := fmt.Sprintf("%d, %d, %d", float32(x)/1000000, float32(y)/1000000, float32(z)/1000000)
		//var test = []byte(dataTest)
		//senseCharacteristic.Write([]byte(dataSense))
		// println("DATA:")
		//println(test)
		//println("Acceleration:", float32(x)/1000000, float32(y)/1000000, float32(z)/1000000)
	}

	// for {
	// 	if !accel.Connected() {
	// 		println("LSM6DS3TR not connected")
	// 		time.Sleep(time.Second)
	// 		continue
	// 	}
	// 	x, y, z, _ := accel.ReadAcceleration()
	// 	println("Acceleration:", float32(x)/1000000, float32(y)/1000000, float32(z)/1000000)
	// 	x, y, z, _ = accel.ReadRotation()
	// 	println("Gyroscope:", float32(x)/1000000, float32(y)/1000000, float32(z)/1000000)
	// 	x, _ = accel.ReadTemperature()
	// 	println("Degrees C", float32(x)/1000, "\n\n")
	// 	time.Sleep(time.Millisecond * 100)
	// }

	// led := machine.LED
	// led.Configure(machine.PinConfig{Mode: machine.PinOutput})
	// for {
	// 	led.Low()
	// 	time.Sleep(time.Millisecond * 2000)
	// 	println("OFF")

	// 	led.High()
	// 	time.Sleep(time.Millisecond * 2000)
	// 	println("ON")
	// }
}

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