#include //SparkFun Haptic Motor Driver Library 
#include 
#include //I2C library 

SFE_HMD_DRV2605L HMD; //Create haptic motor driver object 
DRV2605LEVM_MD multi_drv; //Create multi motor driver object 

uint8_t status_reg_result;
uint8_t control3_reg_result;

uint8_t reg_data = 0xA8; // Hex code for choosing unsigned int for RTP
//uint8_t reg_data = 0xA0; // Hex code for choosing signed int for RTP

uint8_t motor_speed = 0x00;

// Motor selection typdef
motor_sel motor_no = NO_MOTOR;

// Communication variables
int i = 0;
String input_cmd;
int incomingByte = 0; // for incoming serial data

void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
HMD.begin();

// Autocalibrate the motors at the start
Serial.println("Autocalibrating the motor drivers...");
// Start autocalibration by setting the GO bit to 1
HMD.Mode(0x07);
status_reg_result = HMD.readDRV2605L(STATUS_REG);
Serial.print("Autocalibration done! With the status code of ");
Serial.println(status_reg_result, HEX);

// Initialize the motor drivers for RTP mode
Serial.println("Intializing the motor drivers...");
// HMD.Mode(0); // Internal trigger input mode -- Must use the GO() function to trigger playback.
HMD.Mode(0x05); // RTP Mode -- Must use the RTP function to trigger amplitude.
HMD.MotorSelect(0xB6); // LRA motor, 4x Braking, Medium loop gain, 1.365x back EMF gain
HMD.Library(6); //1-5 & 7 for ERM motors, 6 for LRA motors 

//multi_drv.initialize_motors(motor_no);
//status_reg_result = HMD.readDRV2605L(STATUS_REG);
//Serial.print("Motor drivers initialized! With the status code of :");
//Serial.println(status_reg_result, HEX);

// Set the RTP data format type to unsigned int
control3_reg_result = HMD.readDRV2605L(CONTROL3_REG);
Serial.print("Control3 Reg :");
Serial.println(control3_reg_result, HEX);

HMD.writeDRV2605L(CONTROL3_REG, reg_data);
Serial.println("Setting the DATA_FORMAT_RTP to unsigned...");
control3_reg_result = HMD.readDRV2605L(CONTROL3_REG);
Serial.print("Control3 Reg :");
Serial.println(control3_reg_result, HEX);


Serial.println("Setting the CONTROL2_REG to unidirectional...");
HMD.writeDRV2605L(CONTROL2_REG, 0x7F);

Serial.println("Listening the MATLAB commands...");
}

void loop() {

// CHECK FOR COMMUNICATION
if (Serial.available() > 0) {
// read the incoming byte:
input_cmd = Serial.readString();
//Serial.print(input_cmd);
Serial.flush();

// CHECK FOR MOTOR NO
if (input_cmd == "NO_MOTOR\r\n") {
motor_no = NO_MOTOR;
motor_speed = 0;
multi_drv.initialize_motors(motor_no);
Serial.println("No motor is selected");
}
else if(input_cmd == "MOTOR_1\r\n") {
motor_no = MOTOR_1;
motor_speed = 255;
multi_drv.initialize_motors(motor_no);
Serial.println("Motor 1 is selected");
}
else if(input_cmd == "MOTOR_2\r\n") {
motor_no = MOTOR_2;
motor_speed = 255;
multi_drv.initialize_motors(motor_no);
Serial.println("Motor 2 is selected");
}
else if(input_cmd == "MOTOR_3\r\n") {
motor_no = MOTOR_3;
motor_speed = 255;
multi_drv.initialize_motors(motor_no);
Serial.println("Motor 3 is selected");
}
else if(input_cmd == "MOTOR_4\r\n") {
motor_no = MOTOR_4;
motor_speed = 255;
multi_drv.initialize_motors(motor_no);
Serial.println("Motor 4 is selected");
}
else if(input_cmd == "MOTOR_1_2\r\n") {
motor_no = MOTOR_1_2;
motor_speed = 255;
multi_drv.initialize_motors(motor_no);
Serial.println("Motor 1 and 2 are selected");
}
else if(input_cmd == "MOTOR_1_3\r\n") {
motor_no = MOTOR_1_3;
motor_speed = 255;
multi_drv.initialize_motors(motor_no);
Serial.println("Motor 1 and 3 are selected");
}
else if(input_cmd == "MOTOR_1_4\r\n") {
motor_no = MOTOR_1_4;
motor_speed = 255;
multi_drv.initialize_motors(motor_no);
Serial.println("Motor 1 and 4 are selected");
}
else if(input_cmd == "MOTOR_2_3\r\n") {
motor_no = MOTOR_2_3;
motor_speed = 255;
multi_drv.initialize_motors(motor_no);
Serial.println("Motor 2 and 3 are selected");
}
else if(input_cmd == "MOTOR_2_4\r\n") {
motor_no = MOTOR_2_4;
motor_speed = 255;
multi_drv.initialize_motors(motor_no);
Serial.println("Motor 2 and 4 are selected");
}
else if(input_cmd == "MOTOR_3_4\r\n") {
motor_no = MOTOR_3_4;
motor_speed = 255;
multi_drv.initialize_motors(motor_no);
Serial.println("Motor 3 and 4 are selected");
}
else if(input_cmd == "MOTOR_1_2_3_4\r\n") {
motor_no = MOTOR_1_2_3_4;
motor_speed = 255;
multi_drv.initialize_motors(motor_no);
Serial.println("Motor 1 , 2 ,3 and 4 are selected");
}
}


HMD.RTP(motor_speed);
delay(20);
}