#include <Servo.h>

// Program define
#define NUM_CH     6
#define NUM_ACQ  2*NUM_CH + 1
// This define is just to remember where the Capture Compare is enabled
//#define PIN_CPPM  9


#define SYNC_VAL  10000
#define MIN_RANGE   750
#define MAX_RANGE  1650
#define MID_RANGE  1210
#define HEADROOM     10

#define MIN_SERVO_VALUE 5
#define MAX_SERVO_VALUE 175

#define MIN_MOTOR_VALUE 0
#define MAX_MOTOR_VALUE 255

// DEBUG: To print out values uncomment
//#define DEBUG
#define SERVO_0_EN
#define MOTOR_FB_EN


#define F_CLK 1
#ifdef DEBUG
//  #define PRINT_ALL
//  #define PIN_TEST 12
  #define BAUD_RATE 115200
  static uint8_t debug_analog = 1;
#endif

// Variable declaration: volatile because they are changed in the interrupt
static volatile uint16_t cap0[NUM_ACQ];     // Period with PPW capture
static volatile uint16_t cap1[NUM_ACQ];     // Pulse width with PPW capture
static volatile uint16_t nCap;              // Counter used in the interrupt to save array cap0/1
static volatile bool capture_done = false;  // Flag signaling that a whole array of cap0/1 is ready
static uint16_t ch[NUM_CH];                 // Array containing the channels values
uint8_t counter;                            // Counter used to update channel array

#ifdef SERVO_0_EN
  #define PIN_SERVO_0   10
  Servo servo_0;
  int servo0_value = 90; 
#endif

#ifdef MOTOR_FB_EN
  #define PIN_FBM_A_H   12
  #define PIN_FBM_A_L   13
  #define PIN_FBM_B_H    2
  #define PIN_FBM_B_L    4
  int motorFB_value = 0;
#endif

int throttle_value = 0;

int mapValue(int ch_value, int min_range, int max_range, int max_value, int min_value);
int fitChToValue(uint16_t ch_value, uint16_t min_range, uint16_t max_range, uint16_t max_value, uint16_t min_value);

void setup() {
  set_uChip();
  // Add delay to prevent absorbtion from undesired sources
#ifdef SERVO_0_EN
  servo_0.write(servo0_value);
  // Attach to the desired PIN
  servo_0.attach(PIN_SERVO_0);
  delay(500);
#endif
#ifdef MOTOR_FB_EN
  pinMode(PIN_FBM_A_H, OUTPUT);
  pinMode(PIN_FBM_B_H, OUTPUT);
  digitalWrite(PIN_FBM_A_H, LOW);
  digitalWrite(PIN_FBM_B_H, LOW);
  analogWrite(PIN_FBM_A_L, 0);
  analogWrite(PIN_FBM_B_L, 0);
#endif
  set_TTC();
}

void loop() {
  // put your main code here, to run repeatedly:
  if(capture_done)
  {
    noInterrupts();
    update_ch();
// INSERT HERE THE CODE!! In ch[i] you will find the channels values.

    // Translate each channel value into the desired output value
    // Servo values
#ifdef SERVO_0_EN
    // Ch 0 sets the servo direction, with direct connection.
    servo0_value = fitChToValue(ch[0], MIN_RANGE + HEADROOM, MAX_RANGE - HEADROOM, MAX_SERVO_VALUE, MIN_SERVO_VALUE);
#endif

    // Ch[2] sets the motorFB direction and power.
    throttle_value = (int) ch[2];
#ifdef MOTOR_FB_EN    
    if((throttle_value >= MID_RANGE - HEADROOM) &&  (throttle_value <= MID_RANGE + HEADROOM)) // then left everything off
      motorFB_value = 0;
    else
    {
      if(throttle_value > MID_RANGE + HEADROOM)
      {
        motorFB_value = mapValue(throttle_value, MID_RANGE + HEADROOM, MAX_RANGE - HEADROOM, MAX_MOTOR_VALUE, MIN_MOTOR_VALUE);
      }
      else
      { 
        motorFB_value = mapValue(throttle_value, MIN_RANGE + HEADROOM, MID_RANGE - HEADROOM, MAX_MOTOR_VALUE, MIN_MOTOR_VALUE) - MAX_MOTOR_VALUE;
      }
    }
#endif
    // Update all outputs
    //In my case I need to reverse the direction of the servo
#ifdef SERVO_0_EN
    servo_0.write(180 - servo0_value); 
#endif
#ifdef MOTOR_FB_EN
    motorFBwrite();
#endif

#ifdef DEBUG
  #ifdef SERVO_0_EN
   SerialUSB.print("Servo = ");
   SerialUSB.println(servo0_value);
  #endif
  #ifdef MOTOR_FB_EN
   SerialUSB.print("Motor = ");
   SerialUSB.println(motorFB_value);
  #endif
#endif
///////////////////////////////////////////////////////////////////////   
    debug();
    capture_done = false;  
    interrupts();
  }
}

static void motorFBwrite()
{
#ifdef MOTOR_FB_EN
    digitalWrite(PIN_FBM_A_H, LOW);
    digitalWrite(PIN_FBM_B_H, LOW);
    analogWrite(PIN_FBM_A_L, 0);
    analogWrite(PIN_FBM_B_L, 0);
  if(motorFB_value == 0) // Brake
  {
    analogWrite(PIN_FBM_A_L, 255);
    analogWrite(PIN_FBM_B_L, 255);
    return;
  }
  // We keep the pMOSs always on and apply pwm on the nMOSs side
  if(motorFB_value > 0) // forward
  {
    digitalWrite(PIN_FBM_B_H, HIGH);
    analogWrite(PIN_FBM_A_L, motorFB_value);
  }
  else // reverse
  {
    digitalWrite(PIN_FBM_A_H, HIGH);
    analogWrite(PIN_FBM_B_L, -motorFB_value);
  }
#endif
}


// Interrupt handler fro the capture compare routine
void TC3_Handler()
{
  if (TC3->COUNT16.INTFLAG.bit.MC0) 
  {
    REG_TC3_READREQ = TC_READREQ_RREQ |           // Enable a read request
                      TC_READREQ_ADDR(0x18);      // Offset address of the CC0 register
    while (TC3->COUNT16.STATUS.bit.SYNCBUSY);     // Wait for (read) synchronization
    cap0[nCap] = REG_TC3_COUNT16_CC0;              // Copy the period
    // When we get more than NUM_ACQ acquire, we can analyze the resulting data
    if (++nCap == NUM_ACQ) 
    {
      nCap = 0;
      capture_done = true;
    }
  }
    // Check for match counter 1 (MC1) interrupt
  if (TC3->COUNT16.INTFLAG.bit.MC1)           
  {
    REG_TC3_READREQ = TC_READREQ_RREQ |           // Enable a read request
                      TC_READREQ_ADDR(0x1A);      // Offset address of the CC1 register
    while (TC3->COUNT16.STATUS.bit.SYNCBUSY);     // Wait for (read) synchronization
    cap1[nCap] = REG_TC3_COUNT16_CC1;          // Copy the pulse-width
  }
}

static void set_uChip(void)
{
  // Setting uChip pins
  // Turn on bypass pMos, this feature is not necessary in case you have a simple diode
  pinMode(uC7, OUTPUT);
  digitalWrite(uC7, HIGH);
  // Disable boost, we don't need it right now
  pinMode(PIN_BOOST_EN, OUTPUT);
  digitalWrite(PIN_BOOST_EN, LOW);  // Disable boost
  // Set the Buck converter in 5V mode, useless but just in case we power from USB
  pinMode(PIN_VEXT_SELECT, OUTPUT);
  digitalWrite(PIN_VEXT_SELECT, HIGH); // Set 5V on VExt
  // Turn led off
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, LOW);
}


static void set_TTC(void)
{
    
  // Setting pins for program
#ifdef DEBUG
  #ifdef PIN_TEST
    pinMode(PIN_TEST, OUTPUT);
    // Set initial default value
    analogWrite(PIN_TEST, debug_analog);
  #endif
  SerialUSB.begin(BAUD_RATE);
  while(!SerialUSB);
  // Turn LED on to signal that the serial is open
  digitalWrite(LED_BUILTIN,  HIGH);
  SerialUSB.println("Setting up the TC...");
  SerialUSB.println("..turning on PM..");
#endif
  REG_PM_APBCMASK |= PM_APBCMASK_EVSYS;     // Switch on the event system peripheral
#ifdef DEBUG
  SerialUSB.println("..setting the clock generator source..");
#endif
  REG_GCLK_GENDIV = GCLK_GENDIV_DIV(3) |    // Divide the 48MHz system clock by 3 = 16MHz
                    GCLK_GENDIV_ID(5);      // Set division on Generic Clock Generator (GCLK) 5
  while (GCLK->STATUS.bit.SYNCBUSY);        // Wait for synchronization

  REG_GCLK_GENCTRL = GCLK_GENCTRL_IDC |           // Set the duty cycle to 50/50 HIGH/LOW
                     GCLK_GENCTRL_GENEN |         // Enable GCLK 5
                     GCLK_GENCTRL_SRC_DFLL48M |   // Set the clock source to 48MHz
                     GCLK_GENCTRL_ID(5);          // Set clock source on GCLK 5
  while (GCLK->STATUS.bit.SYNCBUSY);              // Wait for synchronization*/

  
  REG_GCLK_CLKCTRL = GCLK_CLKCTRL_CLKEN |         // Enable the generic clock...
                     GCLK_CLKCTRL_GEN_GCLK5 |     // ....on GCLK5
                     GCLK_CLKCTRL_ID_TCC2_TC3;    // Feed the GCLK5 to TCC2 and TC3
  while (GCLK->STATUS.bit.SYNCBUSY);              // Wait for synchronization
    
#ifdef DEBUG
  SerialUSB.println("..setting the input pin connected to CPPM..");
#endif
  // Input on PA19
  PORT->Group[0].PMUX[19/2].bit.PMUXO = MUX_PA19A_EIC_EXTINT3;
  // Set pin to input mode with pull-up resistor enabled
  PORT->Group[0].PINCFG[19].reg=(uint8_t)(PORT_PINCFG_INEN|PORT_PINCFG_PULLEN|PORT_PINCFG_PMUXEN);
#ifdef DEBUG
  SerialUSB.println("..the pulsewidth corresponds to the LOW time..");
#endif
  EIC->CONFIG[0].bit.SENSE3 = EIC_CONFIG_SENSE3_HIGH_Val; // None of RISE,FALL or BOTH work for this
  EIC->EVCTRL.bit.EXTINTEO3 = 1;
  
  // The EIC interrupt is only for verifying the input
  //REG_EIC_INTENSET = EIC_INTFLAG_EXTINT3;
  //NVIC_EnableIRQ(EIC_IRQn);
  while (EIC->STATUS.reg & EIC_STATUS_SYNCBUSY);
  EIC->CTRL.bit.ENABLE = 1;
  while (EIC->STATUS.reg & EIC_STATUS_SYNCBUSY);


#ifdef DEBUG
  SerialUSB.println("..setting TC3 interrupt capture..");
  SerialUSB.println("..giving power to the peripherial..");
#endif

  REG_EVSYS_USER = EVSYS_USER_CHANNEL(1) |                                // Attach the event user (receiver) to channel 0 (n + 1)
                   EVSYS_USER_USER(EVSYS_ID_USER_TC3_EVU);                // Set the event user (receiver) as timer TC3

  REG_EVSYS_CHANNEL = EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT |                // No event edge detection
                      EVSYS_CHANNEL_PATH_ASYNCHRONOUS |                   // Set event path as asynchronous
                      EVSYS_CHANNEL_EVGEN(EVSYS_ID_GEN_EIC_EXTINT_3) |    // Set event generator (sender) as external interrupt 3
                      EVSYS_CHANNEL_CHANNEL(0);                           // Attach the generator (sender) to channel 0
#ifdef DEBUG
  SerialUSB.println("..enable, set the clock generator and assign the clock..");
#endif
  REG_TC3_EVCTRL |= TC_EVCTRL_TCEI |              // Enable the TC event input
                    /*TC_EVCTRL_TCINV |*/         // Invert the event input
                    TC_EVCTRL_EVACT_PPW;          // Set up the timer for capture: CC0 period, CC1 pulsewidth

  REG_TC3_READREQ = TC_READREQ_RREQ |             // Enable a read request
                    TC_READREQ_ADDR(0x06);        // Offset of the CTRLC register
  while (TC3->COUNT16.STATUS.bit.SYNCBUSY);       // Wait for (read) synchronization
  REG_TC3_CTRLC |= TC_CTRLC_CPTEN1 |              // Enable capture on CC1
                   TC_CTRLC_CPTEN0;               // Enable capture on CC0
  while (TC3->COUNT16.STATUS.bit.SYNCBUSY);       // Wait for (write) synchronization
   
  ///////////////////////////////////////////////////////////////////////////////////////////////////////

 
#ifdef DEBUG
  SerialUSB.println("..setting interrupts for capture mode..");
#endif
  NVIC_SetPriority(TC3_IRQn, 0);      // Set the Nested Vector Interrupt Controller (NVIC) priority for TC3 to 0 (highest)
  NVIC_EnableIRQ(TC3_IRQn);           // Connect the TC3 timer to the Nested Vector Interrupt Controller (NVIC)
 
  REG_TC3_INTENSET = TC_INTENSET_MC1 |            // Enable compare channel 1 (CC1) interrupts
                     TC_INTENSET_MC0;             // Enable compare channel 0 (CC0) interrupts
#ifdef DEBUG
  SerialUSB.println("..enable TC..");
#endif 
// Enable TCC
  REG_TC3_CTRLA |= TC_CTRLA_PRESCALER_DIV16 |     // Set prescaler to 16, 16MHz/16 = 1MHz
                   TC_CTRLA_ENABLE;               // Enable TC3
  while (TC3->COUNT16.STATUS.bit.SYNCBUSY);       // Wait for synchronization
#ifdef DEBUG
  SerialUSB.println("..finished TC3 setting procedure.");
#endif 
}

static void update_ch(void)
{
  // Check in the array where there are sync data, the first marks the beginning of recording, the last the ending
    counter = 0;
    for(uint8_t i = 0; i<NUM_ACQ; i++)
    {
      if(cap1[i] > SYNC_VAL)
      {
        counter = 0;
      }
      else
      {
        ch[counter++] = cap1[i]/F_CLK;  
      }
      if(counter == NUM_CH)
        break;
    }  
}

int mapValue(int ch_value, int min_range, int max_range, int max_value, int min_value)
{
  int value = -1;
  if(ch_value < min_range)
      value = min_value;
  else 
  {
    if(ch_value > max_range)
      value = max_value;
    else
      value = (ch_value - min_range)*(max_value - min_value)/(max_range - min_range) + min_value;
  }
  return value;
}


int fitChToValue(uint16_t ch_value, uint16_t min_range, uint16_t max_range, uint16_t max_value, uint16_t min_value)
{
  int value = -1;
  if(ch_value < min_range)
      value = min_value;
  else 
  {
    if(ch_value > max_range)
      value = max_value;
    else
      value = (ch_value - min_range)*(max_value - min_value)/(max_range - min_range) + min_value;
  }
  return value;
}

static void debug(void)
{
  #ifdef DEBUG
    digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
  #ifdef PIN_TEST
    if(debug_analog == 0)
      debug_analog++;
    analogWrite(PIN_TEST,debug_analog++);
  #endif
  #ifdef PRINT_ALL
    for(uint16_t i = 0; i<NUM_ACQ; i++)
    {
      SerialUSB.print("#");
      SerialUSB.print(i);
      SerialUSB.print(F("   "));
      SerialUSB.print(cap0[i]/F_CLK);                      // Output the results
      SerialUSB.print(F("   "));
      SerialUSB.println(cap1[i]/F_CLK); 
     
    }
  #endif
    for(uint8_t i = 0; i < NUM_CH; i++)
    {
      SerialUSB.print("Ch #");
      SerialUSB.print(i);
      SerialUSB.print(" = ");
      SerialUSB.println(ch[i]);                      // Output the results
    }
    SerialUSB.println("/////////////////////////////////////////////////////////////////////////////////////");
#endif

}