#include <Wire.h>
#define PWM6        OCR4D
#define PWM13       OCR4A

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

// COUNTERS
int cycle = 0;

const int micPin1 = A1;        // Microphone 1
const int micPin2 = A0;        // Microphone 2
const int sampleWindow = 50;   // Sample window width in ms
unsigned int sample1, sample2;

// ================================================================================
// code to run once goes here
void setup() {
  Wire.begin();

  Serial.begin(9600);
  pwm613configure();    //sets up PWM signal
  delay(2);
  TCA9548A(1); // connect to motor 1
  initializeDRV2605();  //initializes DRV2605 (the driver that is attached to it)

  delay(10); 
  pulse(0.1, 10);

  // The same for the second motor:
  TCA9548A(2); //initializes the motor, 1 for motor 1, 2 for motor 2 
  initializeDRV2605();  //initializes DRV2605 (the driver that is attached to it)

  delay(10);
  pulse(0.1, 10);
}

// ================================================================================
// active code goes here
void loop()
{
  unsigned long startMillis = millis();

  unsigned int signalMax1 = 0;
  unsigned int signalMin1 = 1023;
  unsigned int signalMax2 = 0;
  unsigned int signalMin2 = 1023;

  while (millis() - startMillis < sampleWindow) {
    sample1 = analogRead(micPin1);
    sample2 = analogRead(micPin2);

    // Mic 1
    if (sample1 < 1023) {
      if (sample1 > signalMax1) signalMax1 = sample1;
      if (sample1 < signalMin1) signalMin1 = sample1;
    }
    
    // Mic 2
    if (sample2 < 1023) {
      if (sample2 > signalMax2) signalMax2 = sample2;
      if (sample2 < signalMin2) signalMin2 = sample2;
    }
  }

  // Peak-to-peak values
  int peakToPeak1 = signalMax1 - signalMin1;
  int peakToPeak2 = signalMax2 - signalMin2;

  // Scale both to 0–50
  int barLength1 = map(peakToPeak1, 0, 1023, 0, 50);
  int barLength2 = map(peakToPeak2, 0, 1023, 0, 50);

  // Print both bars side by side
  Serial.print("Mic1: ");
  for (int i = 0; i < barLength1; i++) Serial.print("#");
  Serial.println();

  Serial.print("Mic2: ");
  for (int i = 0; i < barLength2; i++) Serial.print("#");
  Serial.println();

  Serial.print("Sum: ");
  for (int i = 0; i < barLength1 + barLength2; i++) Serial.print("#");
  Serial.println();

  Serial.println(); // Blank line between updates


  if ((barLength1 > 15) || (barLength2 > 15) || ((barLength1 + barLength2) > 30)){
    TCA9548A(1);
    pulse(1, 20);
    TCA9548A(2);
    pulse(1, 20);
  }

}

// Select I2C BUS
void TCA9548A(uint8_t bus){
  Wire.beginTransmission(0x70);  // TCA9548A address
  Wire.write(1 << bus);          // send byte to select bus
  Wire.endTransmission();
  Serial.print(bus);
}

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

// ================================================================================
// waits for a duration of time defined in ms
void pause(double milliseconds) {
  double us = milliseconds - ((int)milliseconds);
  standbyOnB();
  for (int i = 0; i <= milliseconds; i++) {
    delay(1);
  }
  delayMicroseconds(us * 1000);
}

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

// ================================================================================
// =============================== DRV SETUP ============================= [DRVSET]
// ================================================================================
// sets up the DRV2605 to drive the TacHammer components. It is strongly suggested
// to not edit this section
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);
}

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

void pwm613configure() {
  // 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 D6 (Timer4 D)
void pwmSet6() {
  OCR4D = 0;   // Set PWM value
  DDRD |= _BV(7);    // Set Output Mode D7
  TCCR4C |= 0x09;  // Activate channel D
}

// Set PWM to D13 (Timer4 A)
void pwmSet13() {
  OCR4A = 0;   // Set PWM value
  DDRC |= _BV(7);    // Set Output Mode C7
  TCCR4A = 0x82;  // Activate channel A
}