#include <Wire.h> //Library used for i2c
#include <Servo.h>
#include "wiring_private.h"

//The TCC values are the same for both TCC0 and TCC1
#define I2C_SLAVE_ADDRESS 4
#define TC4_FREQ 500 // 2.73 ms period for the TC4 timer
#define TCC_FREQ 366 // 2.73 ms period for the TCC timers
#define TC4_MIN_DUTY 400
#define TC4_MAX_DUTY 750
#define TCC_MIN_DUTY 400
#define TCC_MAX_DUTY 750
#define MAX_PERIOD 0xFFFF
#define PWM_API_RESOLUTION 10

int pot_value; //value from a 1k pot to be used to debug
uint32_t period; //period value used for the TCC
void setup() {
  //calculate period
  period = 48000000L/TCC_FREQ - 1;
  int i = 0;
  while (period > MAX_PERIOD){
     if (i == 4 || i ==6 || i == 8)
      i++;
     period = 48000000L / TCC_FREQ / (2 << i) - 1;
     i++;
  }
  //Pins 7-10 are outputs to the motors
  //Serial.begin(9600);
  pinMode(7,OUTPUT);
  pinMode(8,OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(9,OUTPUT);
  pinMode(13, OUTPUT);
  
  //Pins 4 & 5 are for i2c communication from the pi
  pinMode(4, INPUT);
  pinMode(5, INPUT);
  Wire.setClock(400000L);
  Wire.begin(I2C_SLAVE_ADDRESS);
  Wire.onReceive(receiveEvent);

/***********************************************************************************
 * Calibrate the ESC to map to the correct high and low duty cycles for each motor *
 ***********************************************************************************/
     pwm(7, TC4_FREQ, TC4_MAX_DUTY);
     pwm(8, TCC_FREQ, TCC_MAX_DUTY);
     pwm(9, TCC_FREQ, TCC_MAX_DUTY);
     pwm(10, TCC_FREQ, TCC_MAX_DUTY);
     delay(2000);
     pwm(7, TC4_FREQ, TC4_MIN_DUTY);
     pwm(8, TCC_FREQ, TCC_MIN_DUTY);
     pwm(9, TCC_FREQ, TCC_MIN_DUTY);
     pwm(10, TCC_FREQ, TCC_MIN_DUTY);
     delay(2000);
/*******************************
 * Motors have been calibrated *
 *******************************/
}

void loop() {
  //Wire.requestFrom(I2C_SLAVE_ADDRESS, 2); //Request 2 bytes from slave device number 4
  //delay(250); //there needs to be a delay here. Not sure how long though. Currently waits 250 us
}

/*
 * This function is used when it receives information on pin 4 from the pi.
 * Each 2 bytes is the throttle for each motor in the order it is received. 
 * First byte is the upper 8 bites for motor 1, second byte is the bottom 8 bits for motor 1, and so on.
 * Then, it sets the pwm for the pin with set duty_cycle.
 * 
 * @param bytes - amount of bytes that it receives
 * 
 */
void receiveEvent(int bytes){
  int pin = 7;
  uint16_t throttle = 0;
  while(Wire.available() > 0){ //loop through all of the bytes
    throttle = Wire.read() << 8;
    throttle = throttle | Wire.read();
    pwmWrite(pin, throttle);
    pin++;
  }
}
/*
 * This function is used to test the motor of a pin. It revs the motor from 0% to 100% throttle
 * 
 * @param pin - number of the pin that the motor is attached to
 */
void revMotor(int pin){
  for(int i = 0; i <= 100; i++){
     pwmWrite(pin, i);
     delay(50);
  }

  for(int i = 100; i >= 0; i--){
    pwmWrite(pin, i);
    delay(50);
  }
}

/*
 * This function is used to write the throttle to a pin.
 * 
 * @param pin - number of the pin that we want to write to
 * @param throttle - the throttle we want to set to
 * 
 * NOTE: this may need to change to directly change the timer values because 
 *       the pwm() function initializes everything together.
 *       Probably do not want to initialize the timers every time
 */
 void pwmWrite(int pin, uint16_t throttle){
    int duty_cycle;
    switch(pin){
      case(7):
        //map the throttle to between the min and max duty cycle values for tc4
        duty_cycle = map(throttle, 0, 65535, TC4_MIN_DUTY, TC4_MAX_DUTY);
        duty_cycle = mapResolution(duty_cycle, 10, 16);
        TC4->COUNT16.CTRLA.bit.ENABLE = 0;
        while(TC4->COUNT16.STATUS.bit.SYNCBUSY);
        TC4->COUNT16.CC[1].reg = (uint32_t)duty_cycle;
        while(TC4->COUNT16.STATUS.bit.SYNCBUSY);
        TC4->COUNT16.CTRLA.bit.ENABLE = 1;
        while(TC4->COUNT16.STATUS.bit.SYNCBUSY);
        break;
      case(8):
        //map the throttle to between the min and max duty cycle values for tcc1
        duty_cycle = map(throttle, 0, 65535, TCC_MIN_DUTY, TCC_MAX_DUTY);
        duty_cycle = map(duty_cycle, 0, (1<<PWM_API_RESOLUTION), 0, period);
        TCC1->CTRLA.bit.ENABLE = 0;
        while(TCC1->SYNCBUSY.reg & TCC_SYNCBUSY_MASK);
        TCC1->CC[1].reg = (uint32_t)duty_cycle;
        while(TCC1->SYNCBUSY.reg & TCC_SYNCBUSY_MASK);
        TCC1->CTRLA.bit.ENABLE = 1;
        while(TCC1->SYNCBUSY.reg & TCC_SYNCBUSY_MASK);        
        break;
      case(9):
        //map the throttle to between the min and max duty cycle values for tcc0
        duty_cycle = map(throttle, 0, 65535, TCC_MIN_DUTY, TCC_MAX_DUTY);
        duty_cycle = map(duty_cycle, 0, (1<<PWM_API_RESOLUTION), 0, period);
        TCC0->CTRLA.bit.ENABLE = 0;
        while(TCC0->SYNCBUSY.reg & TCC_SYNCBUSY_MASK);
        TCC0->CC[1].reg = (uint32_t)duty_cycle;
        while(TCC0->SYNCBUSY.reg & TCC_SYNCBUSY_MASK);
        TCC0->CTRLA.bit.ENABLE = 1;
        while(TCC0->SYNCBUSY.reg & TCC_SYNCBUSY_MASK); 
        break;
      case(10):
        //map the throttle to between the min and max duty cycle values for tcc1
        duty_cycle = map(throttle, 0, 65535, TCC_MIN_DUTY, TCC_MAX_DUTY);
        duty_cycle = map(duty_cycle, 0, (1<<PWM_API_RESOLUTION), 0, period);
        TCC1->CTRLA.bit.ENABLE = 0;
        while(TCC1->SYNCBUSY.reg & TCC_SYNCBUSY_MASK);
        TCC1->CC[0].reg = (uint32_t)duty_cycle;
        while(TCC1->SYNCBUSY.reg & TCC_SYNCBUSY_MASK);
        TCC1->CTRLA.bit.ENABLE = 1;
        while(TCC1->SYNCBUSY.reg & TCC_SYNCBUSY_MASK); 
        break;
      default:
        break;
    }        

 }