#include <string.h>
#include <Arduino.h>
#include <SPI.h>
#if not defined (_VARIANT_ARDUINO_DUE_X_)
//#include <SoftwareSerial.h>
#endif

#include "Adafruit_BLE.h"
#include "Adafruit_BluefruitLE_SPI.h"
#include "Adafruit_BluefruitLE_UART.h"

#include "BluefruitConfig.h"

#include <Wire.h>

#define frontLeftMotor 13
#define backLeftMotor 12
#define frontRightMotor 11
#define backRightMotor 10


//Name your RC here

String BROADCAST_NAME = "ASSpelunker";

String BROADCAST_CMD = String("AT+GAPDEVNAME=" + BROADCAST_NAME);

Adafruit_BluefruitLE_SPI ble(BLUEFRUIT_SPI_CS, BLUEFRUIT_SPI_IRQ, BLUEFRUIT_SPI_RST);

// A small helper
void error(const __FlashStringHelper*err) {
  Serial.println(err);
  while (1);
}

// function prototypes over in packetparser.cpp
uint8_t readPacket(Adafruit_BLE *ble, uint16_t timeout);
float parsefloat(uint8_t *buffer);
void printHex(const uint8_t * data, const uint32_t numBytes);

// the packet buffer
extern uint8_t packetbuffer[];

char buf[60];

void setup(void)
{

  pinMode(frontLeftMotor, OUTPUT);
  pinMode(backLeftMotor, OUTPUT);
  pinMode(frontRightMotor, OUTPUT);
  pinMode(backRightMotor, OUTPUT);

  //digitalWrite(13, HIGH);
  //digitalWrite(10, HIGH);
  //digitalWrite(12, LOW);
  //digitalWrite(11, LOW);
  //Serial.begin(9600);

 Serial.begin(115200);

  /* Initialize the module */
  BLEsetup();

}


bool modeToggle = false;

void loop(void)
{

    //digitalWrite(13, HIGH);
    //digitalWrite(10, HIGH);
    //digitalWrite(12, LOW);
    //digitalWrite(11, LOW);
    // read new packet data
    uint8_t len = readPacket(&ble, BLE_TIMEOUT);
     if (len == 0) return;



    //if no accelerometer, use control pad
    if( !modeToggle ) buttonMode();

}



bool isMoving = false;


bool buttonMode(){ // potentially change to void function

  static unsigned long lastPress = 0;


 // Buttons
  if (packetbuffer[1] == 'B') {

    uint8_t buttnum = packetbuffer[2] - '0';
    boolean pressed = packetbuffer[3] - '0';

  Serial.println(buttnum);
  Serial.println(pressed);
  //Serial.println(isMoving);
    if (pressed) {
      isMoving = true;

      if(buttnum == 7){              // "1" hard pivot LEFT button
    digitalWrite(frontLeftMotor, LOW);
    digitalWrite(backRightMotor, HIGH);
    digitalWrite(backLeftMotor, HIGH);
    digitalWrite(frontRightMotor, LOW);
      }
      if(buttnum == 8){             // "2" hard pivot RIGHT button
    digitalWrite(frontLeftMotor, HIGH);
    digitalWrite(backRightMotor, LOW);
    digitalWrite(backLeftMotor, LOW);
    digitalWrite(frontRightMotor, HIGH);
      }
      if(buttnum == 5){               // "5" both motors backwards
    digitalWrite(frontLeftMotor, HIGH);
    digitalWrite(backRightMotor, HIGH);
    digitalWrite(backLeftMotor, LOW);
    digitalWrite(frontRightMotor, LOW);
      }
      if(buttnum == 6){               // "6" both motors forwards
    digitalWrite(frontLeftMotor, LOW);
    digitalWrite(backRightMotor, LOW);
    digitalWrite(backLeftMotor, HIGH);
    digitalWrite(frontRightMotor, HIGH);
      }
//
      lastPress = millis();

    }
    if(!pressed) {
    isMoving = false;
    digitalWrite(frontLeftMotor, LOW);
    digitalWrite(backRightMotor, LOW);
    digitalWrite(backLeftMotor, LOW);
    digitalWrite(frontRightMotor, LOW);
      }
    return true;
  }
  return false;

}

void BLEsetup(){
  Serial.print(F("Initialising the Bluefruit LE module: "));

  if ( !ble.begin(VERBOSE_MODE) )
  {
    error(F("Couldn't find Bluefruit, make sure it's in Command mode & check wiring?"));
  }
  Serial.println( F("OK!") );

  /* Perform a factory reset to make sure everything is in a known state */
  Serial.println(F("Performing a factory reset: "));
  if (! ble.factoryReset() ){
       error(F("Couldn't factory reset"));
  }

  //Convert the name change command to a char array
  BROADCAST_CMD.toCharArray(buf, 60);

  //Change the broadcast device name here!
  if(ble.sendCommandCheckOK(buf)){
    Serial.println("name changed");
  }
  delay(250);

  //reset to take effect
  if(ble.sendCommandCheckOK("ATZ")){
    Serial.println("resetting");
  }
  delay(250);

  //Confirm name change
  ble.sendCommandCheckOK("AT+GAPDEVNAME");

  /* Disable command echo from Bluefruit */
  ble.echo(true);

  Serial.println("Requesting Bluefruit info:");
  /* Print Bluefruit information */
  ble.info();

  ble.verbose(false);  // debug info is a little annoying after this point!

  /* Wait for connection */
  while (! ble.isConnected()) {
      delay(500);
  }
  // Set Bluefruit to DATA mode
  ble.setMode(BLUEFRUIT_MODE_DATA);
}