//____________________________________________________________________________________________________________________________
//DESCRIPTION-----------------------------------------------------------------------------------------------------------------
/*This code is for the central arduino that controls Solomon's Box in the Solomon's box installation. It communicates with the
other devices via bluetooth and controls 3 stepper motors.*/


//____________________________________________________________________________________________________________________________
//LIBRARIES AND DEFINITIONS---------------------------------------------------------------------------------------------------
#include <ArduinoBLE.h>
#include <Arduino_APDS9960.h>
#include <AccelStepper.h>


//____________________________________________________________________________________________________________________________
//GLOBAL VARIABLES------------------------------------------------------------------------------------------------------------
//MOTOR---------------------------------------------------------------
int motorPos1 = 0; //track motor positions
int motorPos2 = 0;
int motorPos3 = 0;
unsigned char pos1 [4];
unsigned char pos2 [4];
unsigned char pos3 [4];

const int stepPin1 = 2; //assigns pins to motors
const int dirPin1 = 3;
const int stepPin2 = 4;
const int dirPin2 = 5;
const int stepPin3 = 6;
const int dirPin3 = 7;

AccelStepper motor1(1, stepPin1, dirPin1); //creates motor objects
AccelStepper motor2(1, stepPin2, dirPin2);
AccelStepper motor3(1, stepPin3, dirPin3);


//BLE-----------------------------------------------------------------
//Services and Characteristics of other devices
const char* jewelryBoxServiceUuid = "19b10000-e8f2-537e-4f6c-d104768a2008";
const char* motor1CharacteristicUuid = "19b10000-e8f2-537e-4f6c-d104768a1990";
const char* motor2CharacteristicUuid = "19b10000-e8f2-537e-4f6c-d104768a1991";
const char* motor3CharacteristicUuid = "19b10000-e8f2-537e-4f6c-d104768a1992";
BLEService jewelryBoxService;
BLECharacteristic motor1Characteristic;
BLECharacteristic motor2Characteristic;
BLECharacteristic motor3Characteristic;


//OTHER-----------------------------------------------------------------
bool connectedToJewelryBox = false;


//____________________________________________________________________________________________________________________________
//INITIALIZATION--------------------------------------------------------------------------------------------------------------
void setup() {
  Serial.begin(115200);
  while (!Serial);
  delay(3000);

  //APDS--------------------------------------------------------------
  if (!APDS.begin()) {
    Serial.println("* Error initializing APDS9960 sensor!");
  } 

  APDS.setGestureSensitivity(80); 


  //BLE---------------------------------------------------------------
  if (!BLE.begin()) { //initialize bluetooth low energy
    Serial.println("* Starting Bluetooth® Low Energy module failed!");
    while (1);
  }

  //Add characteristics to service, add service to device
  BLE.setLocalName("Solomon's Box (Central)");

  //Advertise
  BLE.setDeviceName("Solomon's Box (Central)");
  BLE.advertise();

  Serial.println("Solomon's Box (Central Device)");
  Serial.println(" ");

  //Motor------------------------------------------------------------
  motor1.setMaxSpeed(1000); // measured in steps per second
  motor1.setAcceleration(500); // measured in steps per second squared
  motor2.setMaxSpeed(1000);
  motor2.setAcceleration(500);
  motor3.setMaxSpeed(1000);
  motor3.setAcceleration(500);


}

//____________________________________________________________________________________________________________________________
//LOOP------------------------------------------------------------------------------------------------------------------------
void loop() {
  alterMotorPos(); 
  motor1.moveTo(motorPos1); // and tell the motor to go there
  motor2.moveTo(motorPos2);
  motor3.moveTo(motorPos3);
  motor1.run(); //commands motor to run
  motor2.run();
  motor3.run();
}


//____________________________________________________________________________________________________________________________
//HELPER FUNCTIONS------------------------------------------------------------------------------------------------------------

void alterMotorPos() {
  if (!Serial.available()) {
    return;
  }
  char num = Serial.read();
  //Serial.println(num);
  if (num == '1') {
    //Serial.println("1 detected");
    motorPos1 += 300;
    motorPos2 += 500;
    motorPos3 += 600;
  }

  if (num == '2') {
    //Serial.println("2 detected");
    motorPos1 += 700;
    motorPos2 += 800;
    motorPos3 += 650;
  }

  if (num == '3') {
    //Serial.println("3 detected");
    motorPos2 += 80;
  }

  //Debugging
  // Serial.print("motorPos1: ");
  // Serial.print(motorPos1);
  // Serial.print("                ");
  // Serial.print("motorPos2: ");
  // Serial.print(motorPos2);
  // Serial.print("                ");
  // Serial.print("motorPos3: ");
  // Serial.println(motorPos3);
}