#include <SoftwareSerial.h>
#include <Wire.h>
#define PWM13       OCR4A

SoftwareSerial Bluetooth(10, 9); // TX | RX OP HC 05 MODULE

const unsigned long connectionTimeout = 5000; // 5 seconds
unsigned long lastConnectionTime = 0;
bool isConnected = false;

//  SETUP I2C
byte DRV = 0x5A;    //DRV2605 slave address
byte ModeReg = 0x01;

void setup() {
  // put your setup code here, to run once:
  Wire.begin();
  Serial.begin(38400);
  Serial.println("HC-05 AT Commands");
  Serial.println("---------------------------------");
  Serial.println("| Function               | Command          | Response            | Parameter");
  Serial.println("|------------------------|------------------|---------------------|--------------------------");
  Serial.println("| Test                   | AT               | OK                  | None");
  Serial.println("| Name                   | AT+NAME?         | +NAME:HC-05, OK     | HC-05 (Default)");
  Serial.println("| Password               | AT+PSWD?         | +PSWD:1234, OK      | 1234 (Default)");
  Serial.println("| Change Name            | AT+NAME=<name>   | OK                  | None");
  Serial.println("| Change Password        | AT+PSWD=<pswd>   | OK                  | None");
  Serial.println("| UART                   | AT+UART?         | +UART:38400,0,0     | 38400 (Default)");
  Serial.println("| Connection mode        | AT+CMODE?        | OK                  | 0 (Default)");
  Serial.println("| Connection mode set    | AT+CMODE=<mode>  | OK                  | 0 (Connect to a specified address) >> | 1 (Connect to any available address)");
  Serial.println("| Find Address           | AT+ADDR?         | +ADDR:<addr>, OK    | None");
  Serial.println("| Connect to Address     | AT+LINK=<addr>   | OK                  | Replace the : with ,");
  Serial.println("| Reset Device           | AT+RESET         | OK                  | None");
  Serial.println("| Restore Factory Default| AT+ORGL          | OK                  | None");
  Serial.println("");
  Serial.println("Enter AT commands to start:");
  Bluetooth.begin(38400);  // HC-05 default speed in AT command mode
  pwm13configure();    //sets up PWM signal
  delay(2);
  initializeDRV2605();  //initializes DRV2605
}

void loop()
{
  // Read from HC-05 and send data to the Arduino Serial Monitor
  // if (Bluetooth.available())
  //   Serial.write(Bluetooth.read());
  String incomingMessage = ""; // Initialize the incoming message string

  if (Bluetooth.available()) {
    String incomingMessage = Bluetooth.readStringUntil('\n'); // Read the incoming message
    incomingMessage.trim();
    Serial.print("Received: ");
    Serial.println(incomingMessage); // Print the received message

    if (incomingMessage == "connected") {
      isConnected = true;
      lastConnectionTime = millis();
      Serial.print("Connection established");
    }
  }
  

  if (millis() - lastConnectionTime > connectionTimeout) {
    isConnected = false;
  }

  if (!isConnected) {
    Serial.println("Connection lost, vibrating motor...");
    pulse(1, 10); // Activate motor
  }

  delay(100);

  // Read from Arduino Serial Monitor and send to the HC-05
  if (Serial.available())
    Bluetooth.write(Serial.read());

  Bluetooth.println("ConnectedToSlave");  
}

void pulse(double intensity, double milliseconds) 
{
  int minimumint = 140;
  int maximumint = 255;
  int pwmintensity = (intensity * (maximumint - minimumint)) + minimumint;
  standbyOffB();
  PWM13 = pwmintensity;
  usdelay(milliseconds);
  standbyOnB();
}


void usdelay(double time) 
{
  double us = time - ((int)time);
  for (int i = 0; i <= time; i++) 
  {
    delay(1);
  }
  delayMicroseconds(us * 1000);
}


void standbyOnB() 
{
  Wire.beginTransmission(DRV);
  Wire.write(ModeReg);            // sets register pointer to the mode register (0x01)
  Wire.write(0x43);               // Puts the device pwm mode
  Wire.endTransmission();
}

void standbyOffB() 
{
  Wire.beginTransmission(DRV);
  Wire.write(ModeReg);               // sets register pointer to the mode register (0x01)
  Wire.write(0x03);               // Sets Waveform Mode to pwm
  Wire.endTransmission();
}

void initializeDRV2605() {
  Wire.beginTransmission(DRV);
  Wire.write(ModeReg);            // sets register pointer to the mode register (0x01)
  Wire.write(0x00);               // clear standby
  Wire.endTransmission();

  Wire.beginTransmission(DRV);
  Wire.write(0x1D);              // sets register pointer to the Libarary Selection register (0x1D)
  Wire.write(0xA8);              // set RTP unsigned
  Wire.endTransmission();

  Wire.beginTransmission(DRV);
  Wire.write(0x03);
  Wire.write(0x02);              // set to Library B, most aggresive
  Wire.endTransmission();

  Wire.beginTransmission(DRV);
  Wire.write(0x17);               // sets full scale reference
  Wire.write(0xff);               //
  Wire.endTransmission();

  Wire.beginTransmission(DRV);
  Wire.write(ModeReg);               // sets register pointer to the mode register (0x01)
  Wire.write(0x03);               // Sets Mode to pwm
  Wire.endTransmission();

  delay(100);

}

/*
DEAR STUDENTS, PLEASE DO NOT MODIFY THIS SECTION
iF YOU NEED ANY ASSISTANCE, PLEASE NOTIFY THE TA OR THE MAIN LECTURER
*/

// Configure the PWM clock
#define PWM12k  5   //  11719 Hz

void pwm13configure() {
  // TCCR4A configuration
  TCCR4A = 0;

  // TCCR4B configuration
  TCCR4B = PWM12k;

  // TCCR4C configuration
  TCCR4C = 0;

  // TCCR4D configuration
  TCCR4D = 0;

  // TCCR4D configuration
  TCCR4D = 0;

  // PLL Configuration
  // Use 96MHz / 2 = 48MHz
  PLLFRQ = (PLLFRQ & 0xCF) | 0x30;
  // PLLFRQ=(PLLFRQ&0xCF)|0x10; // Will double all frequencies

  // Terminal count for Timer 4 PWM
  OCR4C = 255;

  //pwmSet6();
  pwmSet13();
}


// Set PWM to D13 (Timer4 A)
void pwmSet13() {
  OCR4A = 0;  
  DDRC |= _BV(7); 
  TCCR4A = 0x82; 
}