
#include <dsp.h>
#include <stdio.h>
#include "xc.h"
#include "UART.h"
#include "config.h"
#include "extern.h"
#include "init.h"
#include "can.h"



_FOSCSEL(FNOSC_FRC)
_FOSC(FCKSM_CSECMD & OSCIOFNC_ON)
_FWDT(FWDTEN_OFF)
_FPOR(FPWRT_PWR128)  
_FICD(ICS_PGD1 & JTAGEN_OFF)



void (*getErrLoc(void))(void);  // Get Address Error Loc
void (*errLoc)(void);           // Function Pointer   


int main(void) {
    int j;
    char s[256];
    float last_time = 0;
    
    /* Configure Oscillator to operate the device at 40Mhz
       Fosc= Fin*M/(N1*N2), Fcy=Fosc/2
       Fosc= 7.37*(43)/(2*2)=79.2275Mhz for Fosc, Fcy = 39.61375Mhz */
    /* Configure PLL prescaler, PLL postscaler, PLL divisor */
    PLLFBD = 41; /* M = PLLFBD + 2 */
    CLKDIVbits.PLLPOST = 0; /* N1 = 2 */
    CLKDIVbits.PLLPRE = 0; /* N2 = 2 */
    __builtin_write_OSCCONH(0x01); /* New Oscillator selection FRC w/ PLL */
    __builtin_write_OSCCONL(0x01); /* Enable Switch */

    while (OSCCONbits.COSC != 0b001); /* Wait for Oscillator to switch to FRC w/ PLL */
    while (OSCCONbits.LOCK != 1); /* Wait for Pll to Lock */

//Set up UARTS
    //InitU2();  //RS-485
    InitU1();  //RS-232

//init TMR1 to interrupt at 25Hz
    _T1IP = 2;   //Set to lower priority than all the rest.
    TMR1 = 0;  //Clear the timer
    PR1 = 2*12378-1;  //Should give a 25Hz interrupt at 39.6175MHz clock
    T1CON = 0x8020;  //enabled, prescaler 1:64, internal clock
    _T1IF = 0;  //Clear Interrupt
    _T1IE = 0;  //Keep Interrupt disabled until everything is set up.

// init TMR3 to provide the timebase for interrupt that starts the AIO process
    T3CON = 0x8000;
    PR3 = 283-1;  //Rolls over at 139 kHz and adc sampling.  Results in control routines running very close to 4X273.058Hz, which results in
    _T3IF = 0;  //Clear interrupt
    _T3IE = 0;  //Keep Interrupt disabled.


//Setup digital outputs
    TRISA = 0xffff;
    TRISAbits.TRISA9 = 1;   //Don't make this an output, it's connected to ground as Vref-.
    TRISAbits.TRISA10 = 1;  //Don't make this an output, it's connected to 3.3V as Vref+.

    TRISAbits.TRISA0 = 0;  //RA0  -- Not connected to anything, turned on here for Explorer 16 testing.
    TRISAbits.TRISA1 = 0;  //RA1  -- Not connected to anything, turned on here for Explorer 16 testing.

    TRISD = 0xffff;
    TRISDbits.TRISD0 = 0;  //CAN loopback pin, set low to make CAN work properly, or leave it an input?
    TRISDbits.TRISD2 = 0;  //Gas Bypass enable
    TRISDbits.TRISD3 = 0;  //Stepper Enable
    TRISDbits.TRISD12 = 0;  //Laser Rangefinder enable

    TRISDbits.TRISD15 = 0;  //UART1 RTS

    TRISE = 0xffff;
    TRISEbits.TRISE0 = 0;  //PWM1L
    TRISEbits.TRISE1 = 0;  //PWM1H
    TRISEbits.TRISE2 = 0;  //PWM2L
    TRISEbits.TRISE3 = 0;  //PWM2H
    TRISEbits.TRISE4 = 0;  //PWM3L
    TRISEbits.TRISE5 = 0;  //PWM3H
    TRISEbits.TRISE6 = 0;  //PWM4L

    TRISG = 0xffff;
    TRISGbits.TRISG15 = 0;  //EM cable tester enable

    PORTDbits.RD0 = 0;  //CAN loopback pin, set low to make CAN work properly, or leave it an input?


//    while(1)
//    {
//        PORTEbits.RE0 = 1;
//        PORTEbits.RE1 = 1;
//        for(j = 0;j<30000;j++);
//        PORTEbits.RE0 = 0;
//        PORTEbits.RE1 = 0;
//        for(j = 0;j<30000;j++);
//
//    }


//Set up DMA channel and ADC
    InitDma0();  //Argument is number of samples to transfer per interrupt.
    InitADC_WithIntrpt();  

//init CAN bus
    InitCAN1();  //Setup CAN1
    InitDma12();  //Setup two DMA channels for use with CAN1
    for(j=0;j<ECAN1_NUM_TX_BUFFERS;j++)  //Setup CAN messages type and frame type for each message.
      {
      message[j].message_type = CAN_MSG_DATA; //data message
      message[j].frame_type = CAN_FRAME_EXT; //extended (29 bit identifier)
      }

// Can Test by filling in data and sending one message
//    message[0].id = assemble_packetID(CAN_SRCID, 0, 0);
//    message[0].data[0] = 0xde;
//    message[0].data[1] = 0xad;
//    message[0].data[2] = 0xbe;
//    message[0].data[3] = 0xef;
//    message[0].data[4] = 0xaa;
//    message[0].data[5] = 0xbb;
//    message[0].data[6] = 0xcc;
//    message[0].data[7] = 0xdd;
//    message[0].data_length = 8;
//    assembleCAN_Packet(0, &message[0]);
//    C1TR01CONbits.TXREQ0=1;   /* set the message for transmission */

  /*
  for (k=-32768;k<=32767;k++)
    {
    sprintf(s,"%d  %d   %d  \r\n",(int) k,((int)k)/2,LookupDbarMin((int) k));
    putsU2(s);
    }
 * */



// Enable Timer interrupts now that everything is set up.
    _T1IE = 1;  //Enable 25Hz Interrupt, _T1Interrupt gets called at the beginning of each period.

    while (1) {
      if(time > last_time)  //Wait for time to be updated.
      {
    
        
       last_time = time;
       if(raw == 1)
         {
           sprintf(s,"%f   %u  %u  %u  %u  %u  %u  %u  %u    %d/%d/%d %d/%d/%d  %d  %d/%d/%d \r\n",
                   time,AN0_ADC,AN1_ADC,AN10_ADC,AN11_ADC,AN16_ADC,AN17_ADC,AN18_ADC,AN19_ADC,
                    PORTB & 0x0008 ? 1: 0,PORTB & 0x0010 ? 1: 0,PORTB & 0x0020 ? 1: 0,
                    PORTD & 0x0010 ? 1: 0,PORTD & 0x0020 ? 1: 0,PORTD & 0x0040 ? 1: 0,
                    PORTD & 0x2000 ? 1: 0,
                    PORTG & 0x1000 ? 1: 0,PORTG & 0x2000 ? 1: 0,PORTG & 0x4000 ? 1: 0
                 );

         }
       else
         {
          sprintf(s,"%f   %f  %f  %f  %f  %f  %f  %f  %f     %d/%d/%d %d/%d/%d  %d  %d/%d/%d \r\n",
                    time,
                    3.3*AN0_ADC/65535,
                    3.3*AN1_ADC/65535,
                    3.3*AN10_ADC/65535,
                    3.3*AN11_ADC/65535,
                    3.3*AN16_ADC/65535,
                    3.3*AN17_ADC/65535,
                    3.3*AN18_ADC/65535,
                    3.3*AN19_ADC/65535,
                    PORTB & 0x0008 ? 1: 0,PORTB & 0x0010 ? 1: 0,PORTB & 0x0020 ? 1: 0,
                    PORTD & 0x0010 ? 1: 0,PORTD & 0x0020 ? 1: 0,PORTD & 0x0040 ? 1: 0,
                    PORTD & 0x2000 ? 1: 0,
                    PORTG & 0x1000 ? 1: 0,PORTG & 0x2000 ? 1: 0,PORTG & 0x4000 ? 1: 0
                 );
          }
       putsU1(s);  //Send out RS-232 Port
       //putsU2(s);  //Send out RS-485 Port
      }
    }
    return 0;
}  //main
