From 45baea10486b6f28ef38876f07db2d3c6e00cc3f Mon Sep 17 00:00:00 2001
From: Arsen Musayelyan <moussaelianarsen@gmail.com>
Date: Fri, 22 Oct 2021 12:59:51 -0700
Subject: [PATCH] Implement Motion Service

---
 infinitime.go | 136 ++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 136 insertions(+)

diff --git a/infinitime.go b/infinitime.go
index a59afad..84b4ca0 100644
--- a/infinitime.go
+++ b/infinitime.go
@@ -17,6 +17,8 @@ const BTName = "InfiniTime"
 const (
 	NewAlertChar    = "00002a46-0000-1000-8000-00805f9b34fb"
 	NotifEventChar  = "00020001-78fc-48fe-8e23-433b3a1942d0"
+	StepCountChar   = "00030001-78fc-48fe-8e23-433b3a1942d0"
+	MotionValChar   = "00030002-78fc-48fe-8e23-433b3a1942d0"
 	FirmwareVerChar = "00002a26-0000-1000-8000-00805f9b34fb"
 	CurrentTimeChar = "00002a2b-0000-1000-8000-00805f9b34fb"
 	BatteryLvlChar  = "00002a19-0000-1000-8000-00805f9b34fb"
@@ -28,6 +30,8 @@ type Device struct {
 	device          *device.Device1
 	newAlertChar    *gatt.GattCharacteristic1
 	notifEventChar  *gatt.GattCharacteristic1
+	stepCountChar   *gatt.GattCharacteristic1
+	motionValChar   *gatt.GattCharacteristic1
 	fwVersionChar   *gatt.GattCharacteristic1
 	currentTimeChar *gatt.GattCharacteristic1
 	battLevelChar   *gatt.GattCharacteristic1
@@ -277,6 +281,10 @@ func (i *Device) resolveChars() error {
 			i.newAlertChar = char
 		case NotifEventChar:
 			i.notifEventChar = char
+		case StepCountChar:
+			i.stepCountChar = char
+		case MotionValChar:
+			i.motionValChar = char
 		case FirmwareVerChar:
 			i.fwVersionChar = char
 		case CurrentTimeChar:
@@ -330,6 +338,40 @@ func (i *Device) BatteryLevel() (uint8, error) {
 	return uint8(battLevel[0]), nil
 }
 
+func (i *Device) StepCount() (uint32, error) {
+	if !i.device.Properties.Connected {
+		return 0, nil
+	}
+	stepCountData, err := i.stepCountChar.ReadValue(nil)
+	if err != nil {
+		return 0, err
+	}
+	return binary.LittleEndian.Uint32(stepCountData), nil
+}
+
+type MotionValues struct {
+	X int16
+	Y int16
+	Z int16
+}
+
+func (i *Device) Motion() (MotionValues, error) {
+	out := MotionValues{}
+	if !i.device.Properties.Connected {
+		return out, nil
+	}
+	motionVals, err := i.motionValChar.ReadValue(nil)
+	if err != nil {
+		return out, nil
+	}
+	motionValReader := bytes.NewReader(motionVals)
+	err = binary.Read(motionValReader, binary.LittleEndian, &out)
+	if err != nil {
+		return out, err
+	}
+	return out, nil
+}
+
 func (i *Device) HeartRate() (uint8, error) {
 	if !i.device.Properties.Connected {
 		return 0, nil
@@ -407,6 +449,100 @@ func (i *Device) WatchBatteryLevel() (<-chan uint8, error) {
 	return out, nil
 }
 
+func (i *Device) WatchStepCount() (<-chan uint32, func(), error) {
+	if !i.device.Properties.Connected {
+		return make(<-chan uint32), nil, nil
+	}
+	// Start notifications on step count characteristic
+	err := i.stepCountChar.StartNotify()
+	if err != nil {
+		return nil, nil, err
+	}
+	// Watch properties of step count characteristic
+	ch, err := i.stepCountChar.WatchProperties()
+	if err != nil {
+		return nil, nil, err
+	}
+	out := make(chan uint32, 2)
+	currentStepCount, err := i.StepCount()
+	if err != nil {
+		return nil, nil, err
+	}
+	out <- currentStepCount
+	cancel, done := cancelFunc()
+	go func() {
+		// For every event
+		for event := range ch {
+			select {
+			case <-done:
+				close(out)
+				close(done)
+				i.stepCountChar.StopNotify()
+				return
+			default:
+				// If value changed
+				if event.Name == "Value" {
+					// Send step count to channel
+					out <- binary.LittleEndian.Uint32(event.Value.([]byte))
+				}
+			}
+		}
+	}()
+	return out, cancel, nil
+}
+
+func (i *Device) WatchMotion() (<-chan MotionValues, func(), error) {
+	if !i.device.Properties.Connected {
+		return make(<-chan MotionValues), nil, nil
+	}
+	// Start notifications on motion characteristic
+	err := i.motionValChar.StartNotify()
+	if err != nil {
+		return nil, nil, err
+	}
+	// Watch properties of motion characteristic
+	ch, err := i.motionValChar.WatchProperties()
+	if err != nil {
+		return nil, nil, err
+	}
+	out := make(chan MotionValues, 2)
+	motionVals, err := i.Motion()
+	if err != nil {
+		return nil, nil, err
+	}
+	out <- motionVals
+	cancel, done := cancelFunc()
+	go func() {
+		// For every event
+		for event := range ch {
+			select {
+			case <-done:
+				close(out)
+				close(done)
+				i.motionValChar.StopNotify()
+				return
+			default:
+				// If value changed
+				if event.Name == "Value" {
+					// Read binary into MotionValues struct
+					binary.Read(bytes.NewReader(event.Value.([]byte)), binary.LittleEndian, &motionVals)
+					// Send step count to channel
+					out <- motionVals
+				}
+			}
+
+		}
+	}()
+	return out, cancel, nil
+}
+
+func cancelFunc() (func(), chan struct{}) {
+	done := make(chan struct{}, 1)
+	return func() {
+		done <- struct{}{}
+	}, done
+}
+
 // SetTime sets the watch's time using the Current Time Service
 func (i *Device) SetTime(t time.Time) error {
 	if !i.device.Properties.Connected {