/*
 * File:   main.c
 * Author: hamilton
 *
 * Created on November 4, 2013, 4:11 PM
 */

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




_FOSCSEL(FNOSC_FRC)  // FNOSC<2:0> = 000(FRC oscillator)  IES0 = 1  (Start with FRC and then switch to user selected clock when ready)
_FOSC(FCKSM_CSECMD & OSCIOFNC_ON)  // FCKSM<1:0> = 01 (clock switching enable, fail safe clock monitor disabled)   OSCIOFNC = 0 (OSC2 pin is GPIO)     POSCMD<1:0> = 11  (Primary oscillator disabled)
_FWDT(FWDTEN_OFF)  // FWDTEN = 0(watchdog timer software enabled/disabled)  WINDIS = 1 (watchdog timer in non-window mode)  PLLKEN = 1 (clock switch to pll will wait for lock signal)  WDTPRE = 1 (1:128 watchdog prescaler)  WDTPOST<3:0> = 1111 (watchdog postscaler = 1:32768)
_FPOR(FPWRT_PWR128)  //PWMPIN = 1 (MCPWM pins controlled by PORT register at device reset)   HPOL = 1 (MCPWM high side pins are active high)  LPOL = 1 (MCPWM low side pins are active high) FPWRT<2:0> = 11 (power on reset timer value = 128ms)
_FICD(ICS_PGD1 & JTAGEN_OFF) // JTAGEN = 0 (JTAG disabled)   ICS<1:0> = 11  (ICD communicate on PGEC1 and PGED1
        
fractional abcCoefficient_Heading[3] __attribute__((section(".xbss")));
fractional controlHistory_Heading[3] __attribute__((section(".ybss"), far)); //Use this with small data memory model (the default)
        
    float PH1PWM_DC = 0;
    float PH2PWM_DC = 0;
    float PH3PWM_DC = 0;
    const int MC_PWM_Period = 0x7fff;


    int step;
    unsigned int halls = 0;
    unsigned int pos;

    void SampADC(int ch);
    void ScanADC( void );

  char ch,cl;


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


int main(void) {


    
    /* Configure Oscillator to operate the device at ~10Mhz
       Fosc= Fin*M/(N1*N2), Fcy=Fosc/2
       Fosc= 7.37*(43)/(2*8)=19.806875Mhz for Fosc, Fcy = 9.9034375Mhz */
    /* 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 from FRC only, to FRC w/ PLL */
    //__builtin_write_OSCCONL(0x01); /* Request oscillator switch */
    __builtin_write_OSCCONL(0x03); /* Enable secondary (LP) oscillator and Request oscillator 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
    InitU1();
    _U1RXIP = 7;  //Set U1 Recieve interrupt to highest priority since this is occuring asyncronously to everything else.
    
    InitU2();


//init TMR1 to interrupt at specified rate, based on 32768Hz crystal SOSC
    _T1IP = 2;   //Set to lower priority than all the rest.
    TMR1 = 0;  //Clear the timer

#if CAN_PACKET_RATE==25
    PR1 =1311;  //Should give a 25Hz interrupt at 32768Hz clock
    T1CON = 0x8002;  //enabled, prescaler 1:1, external clock (SOSC)
#endif
#if CAN_PACKET_RATE==10
    PR1 =3277;  //Should give a 10Hz interrupt at 32768Hz clock
    T1CON = 0x8002;  //enabled, prescaler 1:1, external clock (SOSC)
#endif

    _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.  
    _T3IF = 0;  //Clear interrupt
    _T3IE = 0;  //Keep Interrupt disabled.

//Setup digital outputs
    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+.


//    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
    TRISD = 0x6F8E;
    _RD0 = 0;  //Set CAN loopback pin low
    _RD12 = 1;  //Enable rangefinder switch
    _RD6 = 0;  //Enable Drive by setting RD6 low 


    TRISE = 0xff09;
    LATEbits.LATE1 = 1;

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

   // TRISEbits.TRISE0 = 0;  //Make RE0 an output
 //   TRISEbits.TRISE1 = 0;  //Make RE1 an output

   //raw = 1;  //Force Calibration Mode
    
//Set up DMA channel and ADC
    InitDma0();  //Argument is number of samples to transfer per interrupt.
    InitADC_WithIntrpt();  


//init CAN bus
    int j;
    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)
      }

//Setup FIR filters
    FIRDelayInit(&ADC_Filter);

//Set up PID structures.
    Heading_PID.abcCoefficients = &abcCoefficient_Heading[0];
    Heading_PID.controlHistory = &controlHistory_Heading[0];
    PIDInit(&Heading_PID);
    kCoeffs_Heading[0] = Q15(DEFAULT_PROP_GAIN);  //Prop Gain  These gains must be between 0-1, to get
    kCoeffs_Heading[1] = Q15(DEFAULT_INT_GAIN); //Int Gain    higher gain in the system, the output of the PID
    kCoeffs_Heading[2] = Q15(DEFAULT_DER_GAIN);  //Der Gain    controller must be scaled up.
    PIDCoeffCalc(&kCoeffs_Heading[0], &Heading_PID);
    
    
    BangBang_Deadband = ConvertDegreesToFract(BANGBANG_DEADBAND);

// init TMR2 to provide the timebase for the stepper motor step signal.
    T2CON = 0x8000;
    PR2 = Speeds_PR2[speed];
    _T2IF = 0;  //Clear interrupt
    _T2IE = 0;  //Keep Interrupt disabled.


    //Set up output compare for step signal
    OC5CONbits.OCM = 0;  //Toggle Pin on counter compare.  Disabled to start, begins when it's time to send steps (in ISR)
    OC5CONbits.OCSIDL = 0;  //OC will continue in CPU idle mode.
    OC5CONbits.OCTSEL = 0;  //Use Timer 2

    OC5R = 0;

   // IPC6bits.OC5IP = 0x01; // Set Output Compare 5 Interrupt Priority Level
   // IFS1bits.OC5IF = 0; // Clear Output Compare 5 Interrupt Flag
   // IEC1bits.OC5IE = 1; // Enable Output Compare 5 interrupt
    
    Status.status = 0;  //Initialize status union to zeros.

    //Convert Default values from engineering units in config.h to working values.
    DefaultHeading = ConvertDegreesToFract(DEFAULT_HEADING);
    HeadingTarget = ConvertDegreesToFract(DEFAULT_HEADING);
    if (((long) HEADING_TIMEOUT) * CAN_PACKET_RATE < UINT_MAX)
        HeadingTargetTimeout = HEADING_TIMEOUT * CAN_PACKET_RATE;
    else
        HeadingTargetTimeout = UINT_MAX;


    HeadingFilterTimeConstant = DEFAULT_HEADING_FILTER_TIME_CONSTANT;
    HeadingFilterDivisorCounter = HeadingFilterTimeConstant*CAN_PACKET_RATE;

    HeadingControlUpdatePeriod = DEFAULT_CONTROL_UPDATE_PERIOD;
    HeadingControlUpdatePeriodCounter = HeadingControlUpdatePeriod*CAN_PACKET_RATE/10; //Control update period is specified in tenths of seconds.

// Enable Timer interrupts now that everything is set up.
    _T1IE = 1;  //Enable Interrupt, _T1Interrupt gets called at the beginning of each period. (10Hz or 25Hz, depending on CAN rate specified in config.h)

#ifdef DEBUG
    char s[256];
    float last_time = 0;

    iTemp1 = 0;


    while (1) {
      if(time > last_time)  //Wait for time to be updated.
      {

          
      last_time = time;
#ifdef ALLOW_CALIBRATION
       if(raw==1)
   //       sprintf(s,"Cal %f   %d  %d   %d      %d     %f\r\n",time,
   //              iTemp1,iTemp2,iTemp3,iTemp4, (((float) iTemp4)*3.3)/32767
   //            );
           sprintf(s,"%f             %d     %d   \r\n",time,
                   WingPos_Buf,
                   Pressure_Buf
                   );
       else
#endif
   // sprintf(s,"%f %f %f %f   %x \r\n",
      //      time,
     //       IMU_Data.accel[0],IMU_Data.accel[1],IMU_Data.accel[2],IMU_Data.VpeStatus
           // IMU_Data.ang_rate[0],IMU_Data.ang_rate[1],IMU_Data.ang_rate[2]
      //      );

       // sprintf(s,"%f  %d  %d\r\n",time,iTemp1,Nortek_Buff);
       // sprintf(s,"%f  %d\r\n",time,HeadingTarget);

//         sprintf(s,"%f   %d  %d    %f    %d  %d        %d   %d       %d %f   %d   %d   %d\r\n",time,
//                 iTemp1,iTemp2,ConvertFractToDegrees(iTemp2),iTemp3,iTemp4,
//                                                            f_RudderPosition,_Q15abs(iTemp3 - f_RudderPosition),
//                                                            HeadingTarget,ConvertFractToDegrees(HeadingTarget),Status.status & 2,Status.status & 4,OC5CONbits.OCM
//               );
        sprintf(s,"%f     %f     %f \r\n",time,
                ConvertFractToEngUnits(WingPos_Buf,WingPos_cnt,WingPos_eng),
                ConvertFractToEngUnits(Pressure_Buf,Pressure_cnt,Pressure_eng)
               );
      //  sprintf(s,"%f    %d     %d    %d       %d   %f    \r\n",time,
        //        iTemp1,iTemp2,iTemp3,iTemp4,ConvertFractToEngUnits(iTemp4,Pressure_cnt,Pressure_eng)

             //  );

    putsU1(s);


     }
    }
#else  //DEBUG not defined
    while(1);
#endif
    return 0;
}  //main