//This example code is in the Public Domain (or CC0 licensed, at your option.)
//By Evandro Copercini - 2018
//
//This example creates a bridge between Serial and Classical Bluetooth (SPP)
//and also demonstrate that SerialBT have the same functionalities of a normal Serial

#include "BluetoothSerial.h"
#include <CodeRacer.h>

#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

BluetoothSerial SerialBT;
bool motioncontrol = false;

String recvddata = "";
int init_azimuth = 1000;
int azimuth = 0;

int minroll = 20;
int roll = 20;
int maxroll = 90;
int minpitch = 10;
int pitch = 0;
int maxpitch = 90;

int defaultleft = 200;
int left = 0;
int defaultright = 200;
int right = 0;

CodeRacer coderacer;

void setup() {
  Serial.begin(115200);
  SerialBT.begin("ESP32test"); //Bluetooth device name
  Serial.println("The device started, now you can pair it with bluetooth!");
  coderacer.begin();
}



void loop() {
  if (Serial.available()) {
    SerialBT.print(Serial.readString());
  }
  if (SerialBT.available()) {
    Serial.println("New incoming message");
    SerialBT.setTimeout(10);
    recvddata = SerialBT.readStringUntil('\0');
    recvddata.remove(recvddata.length()-2,2);  //remove delimiter from string
    Serial.println(recvddata);
    BTcontrol();
  }
  delay(20);
}

void BTcontrol()
{
  
  
  if(recvddata.startsWith("motion_on"))
  {
    SerialBT.print("motion_ready");
    init_azimuth = 1000;
    motioncontrol = true;
    return;
  }
  
  if(recvddata.startsWith("motion_off"))
  {
    SerialBT.print("motion_disabled");
    motioncontrol = false;
    coderacer.stop_driving();
    return;
  }

  if(recvddata.startsWith("apr="))
  {
    if(motioncontrol == true)
    {
      SerialBT.print(BTmotioncontrol(recvddata));
    }
    else
    {
      SerialBT.print("motion_disabled");
    }
  }

  
}

String BTmotioncontrol(String recvddata)
{
  // split the string ...
  String apr_substr=recvddata.substring(recvddata.indexOf("=")+1);
  String a = apr_substr.substring(0,apr_substr.indexOf(";"));
  String p = apr_substr.substring(apr_substr.indexOf(";")+1,apr_substr.lastIndexOf(";"));
  String r = apr_substr.substring(apr_substr.lastIndexOf(";")+1);

  azimuth = a.toInt();
  pitch = p.toInt();
  roll = r.toInt();
  
  if(init_azimuth == 1000)
  {
    init_azimuth = azimuth;
  }

  azimuth = azimuth - init_azimuth;
  if(abs(pitch)<minpitch){
    pitch = 0;
    coderacer.stop_driving();
  }
  else
  {
    pitch = pitch;


    if(pitch > 0)
    {
      left = defaultleft+pitch;
      if(left > 256) left = 256;
      right = defaultright+pitch;
      if(right > 256) right = 256;
      coderacer.drive_forward(left,right);
    }
    else
    {
      left = defaultleft-pitch;
      if(left > 256) left = 256;
      right = defaultright-pitch;
      if(right > 256) right = 256;
      coderacer.drive_backward(left,right);
    }
  }
    
  
  if(abs(roll)<minroll){
    roll = 0;
  }
  /*
  int defaultleft = 240;
  int left = 0;
  int defaultright = 240;
  int right = 0;
  */

  Serial.println(azimuth);
  Serial.println(pitch);
  Serial.println(roll);
  
  return "motion_values_ok";
}