//#define NU32_STANDALONE  // uncomment if program is standalone, not bootloaded
#include "NU32.h"          // plib.h, config bits, constants, funcs for startup and UART
#define MAX_MESSAGE_LENGTH 200

void init_analog(void);
int read_analog(int pin);

void init_ultrasonic(void);
int read_ultrasonic(void);

void init_rcservo(void);
void set_rcservo(int degrees);

int main(void) {
  char message[MAX_MESSAGE_LENGTH];
  int pot_value[140]; // 140 different angles, from 20 degrees to 160
  int ultrasonic_value[140];
  int angle = 20;

  NU32_Startup(); // cache on, min flash wait, interrupts on, LED/button init, UART init

  init_analog();
  init_ultrasonic();
  init_rcservo();

  while (1) {
    if (NU32USER == 0) { // if the user button is pushed
      // run thru the servo positions
      for (angle=20;angle<160;angle++) {
        set_rcservo(angle);
        // save the readings into an array
        pot_value[angle-20] = read_analog(15)/2;
        ultrasonic_value[angle-20] = read_ultrasonic()/235;
        // print them out so you can see what it saw
        sprintf(message, "%d %d\n", pot_value[angle-20], ultrasonic_value[angle-20]);
        NU32_WriteUART1(message);
      }

      // now you have the data in the arrays pot_value and ultrasonic_value
      // find the middle of the thing you saw, and set the servo to point at it
    }

    // go to the middle of the range
    set_rcservo(90);

    // send over the current readings
    sprintf(message, "%d %d\n", read_analog(15)/2, read_ultrasonic()/235);
    NU32_WriteUART1(message);

    // delay just a little
    WriteCoreTimer(0);
    while(ReadCoreTimer() < 400000);

    NU32LED1 = !NU32LED1; // toggle the LEDs
    NU32LED2 = !NU32LED2;
  }
  return 0;
}

void init_analog(void) {
  // Set all AN pins (B0-B15) for analog input
  TRISBSET = 0xFFFF;
  AD1PCFGCLR = 0xFFFF;


  AD1CON1bits.FORM = 0b000; // Select 16-bit integer output
  AD1CON1bits.ASAM = 1; // Auto Sample
  AD1CON1bits.SSRC = 0b111; // Auto convert sampled data to digital
  AD1CON2bits.VCFG = 0b000; // Select the vrefs to be Vdd and Vss

  AD1CON2bits.CSCNA = 1; // Enable ADC Scaning
  AD1CSSL = 0xFFFF; // Select all AN pins for scanning
  AD1CON2bits.SMPI = 16 - 1; // Select 3 samples per interupt
  /* In scanning mode, this number must be at least as big as
   * the number of channels you are scanning/
   */

  AD1CON3bits.ADRC = 1; // Use internal RC clock
  AD1CON3bits.SAMC = 2; // Set sampling time 2*Tad
  AD1CON1bits.ADON = 1; // Turn the ADC on
}

int read_analog(int pin) {
  switch (pin) {
    case 0:
      return ADC1BUF0;
      break;
    case 1:
      return ADC1BUF1;
      break;
    case 2:
      return ADC1BUF2;
      break;
    case 3:
      return ADC1BUF3;
      break;
    case 4:
      return ADC1BUF4;
      break;
    case 5:
      return ADC1BUF5;
      break;
    case 6:
      return ADC1BUF6;
      break;
    case 7:
      return ADC1BUF7;
      break;
    case 8:
      return ADC1BUF8;
      break;
    case 9:
      return ADC1BUF9;
      break;
    case 10:
      return ADC1BUFA;
      break;
    case 11:
      return ADC1BUFB;
      break;
    case 12:
      return ADC1BUFC;
      break;
    case 13:
      return ADC1BUFD;
      break;
    case 14:
      return ADC1BUFE;
      break;
    case 15:
      return ADC1BUFF;
      break;
  }
}

// Put ECHO to  and TRIG to

void init_ultrasonic(void) {
  TRISEbits.TRISE8 = 1; // ECHO to A2, NU32 input pin
  TRISEbits.TRISE9 = 0; // TRIG to A3, NU32 output pin
  LATEbits.LATE9 = 0; // start off
}

// return the time of flight of the ultrasonic signal

int read_ultrasonic(void) {
  int flight_time[5];
  int av_flight = 0;
  int i;
  int start_time;

  // read the sensor 5 times and average the result
  for (i = 0; i < 5; i++) {
    start_time = ReadCoreTimer();

    // pulse the trigger for 20us to send the ultrasonic signals
    LATEbits.LATE9 = 1;
    while (ReadCoreTimer() - start_time < 1600) {
    }
    LATEbits.LATE9 = 0;

    // wait for echo to go high
    while (!PORTEbits.RE8) {
    }
    start_time = ReadCoreTimer();

    // wait until the ECHO pin goes low, or 12ms has passed
    while (PORTEbits.RE8 && (ReadCoreTimer() - start_time < 480000)) {
    }
    flight_time[i] = ReadCoreTimer() - start_time;
  }

  for (i = 0; i < 5; i++) {
    av_flight = av_flight + flight_time[i];
  }

  av_flight = av_flight / 5;

  return av_flight;
}

void init_rcservo(void) {
  // set a 50Hz timer for the PWM signal to the rc servo
  T2CON = 0; // turn off TMR2, T2CON<ON> = 0
  OC1CON = 0; // turn off OC1, OC1CON<ON> = 0
  T2CONbits.TCKPS = 0b111; // set prescaler 1:256, see RM Section 14 pp. 9-10
  TMR2 = 0; // start TMR2 counting from zero
  PR2 = 6249; // set period match value
  /* now TMR2 has a period of (PR2 + 1) / PBCLK * 2^T2CON<TCKPS> = 20ms or 50Hz. */

  OC1CONbits.OCM = 0b110; // enable PWM mode with no fault protection
  /* 0 degrees on the servo is a 0.5ms pulse, or OC1RS = (6250/0.02s)*0.0005s = 157 */
  /* 180 degrees on the servo is a 2.5ms pulse, or OC1RS = (6250/0.02s)*0.0025s = 781 */
  /* 90 degrees on the servo is a 1.5ms pulse, or OC1RS = (6250/0.02s)*0.0015s = 467 */
  /* so DON'T SET OC1RS OUTSIDE THE RANGE 157-781*/
  OC1RS = 467; // duty cycle is OC1RS/(PR2+1)
  OC1R = 467; // set initial PWM duty cycle in counts
  // don't write to OC1R while PWM is running (it's read-only),
  // instead use OC1RS.

  T2CONbits.ON = 1; // turn on TMR2
  OC1CONbits.ON = 1; // turn on OC1 (pin D0)
}

void set_rcservo(int degrees) {
  float set_valuef = degrees * 3.467+157.0; //(degrees-0)*(781-157)/(180-0)+157;
  int set_valuei = set_valuef;

  OC1RS = set_valuei;
}