#include <Wire.h>
#define PWM13       OCR4A

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

byte TCA = 0x70;

int ports[2] = {1,2};

void setup() {
  Wire.begin();
  // while(!Serial){}; // Sequence will not run until we activate the serial port (or monitor)

  Serial.begin(9600); 
  pwm13configure();    //sets up PWM signal
  delay(2);
  initializeDRV2605();  //initializes DRV2605

  for (int i = 0; i < sizeof(ports)/sizeof(int); i++)
  {
    TCA9548A(ports[i]);
    initializeDRV2605();
    delay(10);
    pulse(0.1, 10);
  }
}

void loop() {
  // put your main code here, to run repeatedly:
  while (true)
    {
    TCA9548A(1);
    initializeDRV2605();
    delay(10);
    pumping(100);
    delay(1000);
  
    TCA9548A(2);
    initializeDRV2605();
    delay(10);
    breathing(300);
    delay(1000);

    TCA9548A(1);
    initializeDRV2605();
    delay(10);
    pumping(100);
    delay(1000);

    TCA9548A(2);
    initializeDRV2605();
    delay(10);
    buzzing(50);
    delay(1000);
    }
}

// Basic functions:
/*
Assignment PART 3
We use strong pulses at 100 BPM or 1.667 beats per second
then we pause to give room for breathing air into the lungs (2 weak pulses with larger interval)
*/
void pumping(double BPM)
{
   // bpm to ms
  double pulse_duration = 60.0 / BPM * 1000;
  
  // 100 BPM
  for (int i = 0; i < 30; i++)
  {
    pulse(0.9, pulse_duration);
  }
}

void breathing(double duration)
{
  // breathing air into the lungs
  for (int i = 0; i < 4; i++)
  {
    pulse(0.3, duration);
    delay(1000);
  }
}

void buzzing(double duration)
{
  // breathing air into the lungs
  for (int i = 0; i < 100; i++)
  {
    pulse(0.3, duration);
  }
}

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

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 pause(double milliseconds) 
{
  double us = milliseconds - ((int)milliseconds);
  standbyOnB();
  for (int i = 0; i <= milliseconds; i++) 
  {
    delay(1);
  }
  delayMicroseconds(us * 1000);
}

void vibrate(double frequency, double intensity, double duration, int dutycycle) 
{
  int max_hit = 12;
  int min_hit = 1;
  int crossover = 60;
  int hitduration = 10 * dutycycle / frequency;

  boolean hold = false;
  double delayy;
  delayy = (1 / frequency * 1000) - hitduration;
  double timedown;
  timedown = duration * 1000;

  if (duration == 0)
  {
    hold =  true;
  }

  while (hold)
  {
    pulse(intensity, hitduration);
    pause(delayy);
  }

  while (timedown >= 0 && frequency < crossover)
  {
    pulse(intensity, hitduration);
    pause(3);
    pulse(0.002, delayy-3);

    timedown -= (delayy + hitduration);
  }

  while (timedown >= 0 && frequency >= crossover) 
  {
    pulse(intensity, hitduration);
    pause(delayy);
    timedown -= (delayy + hitduration);
  }
}

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);

}

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 TCA9548A(uint8_t bus){
  Wire.beginTransmission(TCA);
  Wire.write(1 << bus);
  Wire.endTransmission();
}

/*
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; 
}