#include "BLEDevice.h"
#include <SparkFun_TB6612.h>

#define DEV_NAME "SPadXXX"

static BLEUUID serviceUUID("56dcb94f-01c0-4a4c-8f6c-efba1b184569");
static BLEUUID    charUUID("31fd93ba-4461-49ac-8c21-c6e723564c20");

static boolean doConnect = false;
static boolean connected = false;
static boolean doScan = false;
static BLERemoteCharacteristic* pRemoteCharacteristic;
static BLEAdvertisedDevice* myDevice;

const int offsetA = 1;
const int offsetB = 1;

#define AIN1 5
#define BIN1 6
#define AIN2 4
#define BIN2 7
#define PWMA 3
#define PWMB 8
#define STBY 9

Motor motor1 = Motor(AIN1, AIN2, PWMA, offsetA, STBY);
Motor motor2 = Motor(BIN1, BIN2, PWMB, offsetB, STBY);


volatile int8_t left_horizontal = 0;
volatile int8_t left_vertical = 0;
volatile int8_t right_horizontal = 0;
volatile int8_t right_vertical = 0;

static void notifyCallback(
  BLERemoteCharacteristic* pBLERemoteCharacteristic,
  uint8_t* pData,
  size_t length,
  bool isNotify) {
    Serial.printf("%d-%d-%d-%d-%d-%d\n",
    (uint8_t)pData[0],
    (uint8_t)pData[1],
    (int8_t)pData[2],
    (int8_t)pData[3],
    (int8_t)pData[4],
    (int8_t)pData[5]);
    left_horizontal = (int8_t)pData[2];
    left_vertical = (int8_t)pData[3];
    right_horizontal = (int8_t)pData[4];
    right_vertical = (int8_t)pData[5];
    }

class MyClientCallback : public BLEClientCallbacks {
  void onConnect(BLEClient* pclient) {
  }

  void onDisconnect(BLEClient* pclient) {
    connected = false;
    Serial.println("onDisconnect");
  }
};

bool connectToServer() {
    Serial.print("Forming a connection to ");
    Serial.print(myDevice->getAddress().toString().c_str());
    Serial.print(" - ");
    Serial.println(myDevice->getName().c_str());
    
    BLEClient*  pClient  = BLEDevice::createClient();
    Serial.println(" - Created client");

    pClient->setClientCallbacks(new MyClientCallback());

    pClient->connect(myDevice);  
    Serial.println(" - Connected to server");
    pClient->setMTU(517);
  
    BLERemoteService* pRemoteService = pClient->getService(serviceUUID);
    if (pRemoteService == nullptr) {
      Serial.print("Failed to find our service UUID: ");
      Serial.println(serviceUUID.toString().c_str());
      pClient->disconnect();
      return false;
    }
    Serial.println(" - Found our service");


    pRemoteCharacteristic = pRemoteService->getCharacteristic(charUUID);
    if (pRemoteCharacteristic == nullptr) {
      Serial.print("Failed to find our characteristic UUID: ");
      Serial.println(charUUID.toString().c_str());
      pClient->disconnect();
      return false;
    }
    Serial.println(" - Found our characteristic");

    if(pRemoteCharacteristic->canNotify())
      pRemoteCharacteristic->registerForNotify(notifyCallback);

    connected = true;
    return true;
}

class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
  void onResult(BLEAdvertisedDevice advertisedDevice) {
    Serial.print("BLE Advertised Device found: ");
    Serial.println(advertisedDevice.toString().c_str());
    String dn = "";
    if(advertisedDevice.getName().c_str() != NULL)
      dn = String(advertisedDevice.getName().c_str());
    Serial.println(dn);

    if (dn.equals(DEV_NAME) && advertisedDevice.haveServiceUUID() && advertisedDevice.isAdvertisingService(serviceUUID)) {
      BLEDevice::getScan()->stop();
      myDevice = new BLEAdvertisedDevice(advertisedDevice);
      doConnect = true;
      doScan = true;
    }
  }
};


void setup() {
  Serial.begin(115200);
  Serial.println("Starting Arduino BLE Client application...");
  BLEDevice::init("");

  BLEScan* pBLEScan = BLEDevice::getScan();
  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
  pBLEScan->setInterval(1349);
  pBLEScan->setWindow(449);
  pBLEScan->setActiveScan(true);
  pBLEScan->start(5, false);

  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, HIGH);
  delay(1000);
  digitalWrite(LED_BUILTIN, LOW);
}


void loop() {
  if (doConnect == true) {
    if (connectToServer()) {
      Serial.println("We are now connected to the BLE Server.");
      digitalWrite(LED_BUILTIN, HIGH); 
    } else {
      Serial.println("We have failed to connect to the server; there is nothin more we will do.");
    }
    doConnect = false;
  }

  if (connected) {
    String newValue = "Time since boot: " + String(millis()/1000);
    int direction = map(right_horizontal,-128,127,-255,255);
    int speed = map(right_vertical,-128,127,-255,255);
    
    int left_speed  = constrain(speed + direction, -255, 255);
    int right_speed = constrain(speed - direction, -255, 255);
    motor1.drive(right_speed);
    motor2.drive(left_speed);

    pRemoteCharacteristic->writeValue(newValue.c_str(), newValue.length());
  }else if(doScan){
    BLEDevice::getScan()->start(0);
  }
}