/* Sample RC servo input for 4 channels using Input Capture hardware perihperal and interrupts
    Written for Rick Anderson by Brian Schmalz
    11/11/2017
    This software is put in the public domain.
    This sketch is means for a chipKIT Fubarino SD board, but can be easily adpated to any PIC32 based Arduino compatible board.
*/

/* Fubarino SD can only use pins 0, 1, 2, 3, and 10 as Input Capture.
   On any chipKIT board with PPS (like Fubarino Mini) additional setup is necessary to map the Input Capture
   peripherals to speciffic I/O pins, but you have much more flexibility in pin assignments.
*/

#include "Mouse.h"

// Which pins correspond to which RC input channel?
#define RC_INPUT_1 0
#define RC_INPUT_2 1
#define RC_INPUT_3 2
#define RC_INPUT_4 3
// How many RC input channels are there?
#define RC_INPUT_COUNT 4

// These two arrays hold the latest pulse measurements for each RC input channel
volatile uint16_t pulseHighTime[RC_INPUT_COUNT];
volatile uint16_t pulseLowTime[RC_INPUT_COUNT];

/* Four Input Capture Interrupt Service Routines
    These functions get called on each rising and falling edge. They check to see
    if a rising edge or falling edge just happened, then read out the Timer2 value that
    got latched on the edge, and calculate the high and low pulse times, then stick those
    times in the proper array.
*/
void __USER_ISR InputCatpure1_ISR(void) {
  static uint16_t risingEdgeTime = 0;
  static uint16_t fallingEdgeTime = 0;

  clearIntFlag(_INPUT_CAPTURE_1_IRQ);
  if (IC1CONbits.ICBNE == 1)
  {
    if (digitalRead(RC_INPUT_1) == HIGH)
    {
      risingEdgeTime = IC1BUF;
      pulseLowTime[0] = risingEdgeTime - fallingEdgeTime;
    }
    else
    {
      fallingEdgeTime = IC1BUF;
      pulseHighTime[0] = fallingEdgeTime - risingEdgeTime;
    }
  }
}

void __USER_ISR InputCatpure2_ISR(void) {
  static uint16_t risingEdgeTime = 0;
  static uint16_t fallingEdgeTime = 0;

  clearIntFlag(_INPUT_CAPTURE_2_IRQ);
  if (IC2CONbits.ICBNE == 1)
  {
    if (digitalRead(RC_INPUT_2) == HIGH)
    {
      risingEdgeTime = IC2BUF;
      pulseLowTime[1] = risingEdgeTime - fallingEdgeTime;
    }
    else
    {
      fallingEdgeTime = IC2BUF;
      pulseHighTime[1] = fallingEdgeTime - risingEdgeTime;
    }
  }
}

void __USER_ISR InputCatpure3_ISR(void) {
  static uint16_t risingEdgeTime = 0;
  static uint16_t fallingEdgeTime = 0;

  clearIntFlag(_INPUT_CAPTURE_3_IRQ);
  if (IC3CONbits.ICBNE == 1)
  {
    if (digitalRead(RC_INPUT_3) == HIGH)
    {
      risingEdgeTime = IC3BUF;
      pulseLowTime[2] = risingEdgeTime - fallingEdgeTime;
    }
    else
    {
      fallingEdgeTime = IC3BUF;
      pulseHighTime[2] = fallingEdgeTime - risingEdgeTime;
    }
  }
}

void __USER_ISR InputCatpure4_ISR(void) {
  static uint16_t risingEdgeTime = 0;
  static uint16_t fallingEdgeTime = 0;

  clearIntFlag(_INPUT_CAPTURE_4_IRQ);
  if (IC4CONbits.ICBNE == 1)
  {
    if (digitalRead(RC_INPUT_4) == HIGH)
    {
      risingEdgeTime = IC4BUF;
      pulseLowTime[3] = risingEdgeTime - fallingEdgeTime;
    }
    else
    {
      fallingEdgeTime = IC4BUF;
      pulseHighTime[3] = fallingEdgeTime - risingEdgeTime;
    }
  }
}

void setup() {
  Serial.begin(9600);
  delay(3000);
  Serial.println("OK we are starting now");
  Mouse.begin();


  /* Set up each of the four Input Capture modules
     We want them to generate an interrupt on each rising and falling edge of the input
     We also want them all to use Timer2 as their timebase timer
  */
  IC1CON = 0;
  IC1CONbits.ICM = 1;   // Captured and interrupt on every rising and falling edge
  IC1CONbits.ICTMR = 1; // Set to user Timer2
  IC1CONbits.ON = 1;    // Turn IC1 on

  IC2CON = 0;
  IC2CONbits.ICM = 1;   // Captured and interrupt on every rising and falling edge
  IC2CONbits.ICTMR = 1; // Set to user Timer2
  IC2CONbits.ON = 1;    // Turn IC2 on

  IC3CON = 0;
  IC3CONbits.ICM = 1;   // Captured and interrupt on every rising and falling edge
  IC3CONbits.ICTMR = 1; // Set to user Timer2
  IC3CONbits.ON = 1;    // Turn IC3 on

  IC4CON = 0;
  IC4CONbits.ICM = 1;   // Captured and interrupt on every rising and falling edge
  IC4CONbits.ICTMR = 1; // Set to user Timer2
  IC4CONbits.ON = 1;    // Turn IC4 on

  /* Set up Timer2: We want it to simply count up. We set the prescaler to 1:64
      so that the 40MHz PCLK gets divided down to 1.25Mhz. This is nice because
      then it gives us both high and low periods under 65535 for normal RC servo
      times (0-3 ms high time and 17-20ms low time)
  */
  PR2 = 0xFFFF;         // Run from 0 to 0xFFFF
  T2CONbits.TCKPS = 5;  // 1:32 prescale, which means 40MHz/64 or 1.25MHz clock rate
  T2CONbits.TON = 1;    // Turn on Timer2

  // Set all input captures as inputs
  pinMode(RC_INPUT_1, INPUT);
  pinMode(RC_INPUT_2, INPUT);
  pinMode(RC_INPUT_3, INPUT);
  pinMode(RC_INPUT_4, INPUT);
  Serial.println("ready");

  // Set each Input Capture up to use its ISR routine
  setIntVector(_INPUT_CAPTURE_1_VECTOR, InputCatpure1_ISR);
  setIntPriority(_INPUT_CAPTURE_1_VECTOR, 4, 0);
  clearIntFlag(_INPUT_CAPTURE_1_IRQ);
  setIntEnable(_INPUT_CAPTURE_1_IRQ);

  setIntVector(_INPUT_CAPTURE_2_VECTOR, InputCatpure2_ISR);
  setIntPriority(_INPUT_CAPTURE_2_VECTOR, 4, 0);
  clearIntFlag(_INPUT_CAPTURE_2_IRQ);
  setIntEnable(_INPUT_CAPTURE_2_IRQ);

  setIntVector(_INPUT_CAPTURE_3_VECTOR, InputCatpure3_ISR);
  setIntPriority(_INPUT_CAPTURE_3_VECTOR, 4, 0);
  clearIntFlag(_INPUT_CAPTURE_3_IRQ);
  setIntEnable(_INPUT_CAPTURE_3_IRQ);

  setIntVector(_INPUT_CAPTURE_4_VECTOR, InputCatpure4_ISR);
  setIntPriority(_INPUT_CAPTURE_4_VECTOR, 4, 0);
  clearIntFlag(_INPUT_CAPTURE_4_IRQ);
  setIntEnable(_INPUT_CAPTURE_4_IRQ);
}

void loop() {
  static uint32_t lastPrintTime = 0;
  float uSscale = 0.4;

  /* So the way things work, the ISRs will get called and they will constantly update
      the pulseHighTime and pulseLowTime arrays "in the background". In other words the
      mainline code here doesn't ever have to do anything other than simply read out
      the values in the arrays, and those values will always be the most recent pulse
      measurements. If you care about converting the values to real time (like in
      microsconds), the units for these values are in 1.25MHz 'counts'. So 1ms = 1250
      'counts'.
  */
  // To demo things, simply print out all 4 high and low pulse times every half second
  if ((millis() - lastPrintTime) > 500)
  {
    lastPrintTime = millis();
    char pulses[100];
    char micros[100];
    sprintf(pulses, "high1: %05u low1: %05u high2: %05u low2: %05u high3: %05u low3: %05u high4: %05u low4: %05u\n",
            pulseHighTime[0],
            pulseLowTime[0],
            pulseHighTime[1],
            pulseLowTime[1],
            pulseHighTime[2],
            pulseLowTime[2],
            pulseHighTime[3],
            pulseLowTime[3]
           );
    Serial.print(pulses);
    sprintf(micros, "ch1: %05u ch2: %05u ch3: %05u ch4: %05u\n",
            (int) (pulseHighTime[0] * uSscale),
            (int) (pulseHighTime[1] * uSscale),
            (int) (pulseHighTime[2] * uSscale),
            (int) (pulseHighTime[3] * uSscale)
           );

    Serial.print(micros);

  }
  if (   (int) (pulseHighTime[2] * uSscale) > 1500   ) {
    int xReading = map( (int) (pulseHighTime[0] * uSscale), 1000, 2000, -6, 6);
    int yReading = map( (int) (pulseHighTime[1] * uSscale), 1000, 2000, -6, 6);

    Mouse.move(xReading, yReading, 0);
  }
}