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

#include <dsp.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "xc.h"
#include "UART.h"
#include "config.h"
#include "extern.h"
#include "../CommonCode/conv.h"
#include "init.h"
#include "CalibrationConstants.h"
#include "libq.h"
#include "../CommonCode/types.h"
#include "../CommonCode/crc.h"
#include "../CommonCode/i2c.h"
#include "../CommonCode/crit_section.h"

_FOSCSEL(FNOSC_PRIPLL)
//_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) {
    float last_time = 0;
#ifdef DEBUG
    char s[256];
#endif


    /* Configure Oscillator to operate the device at 40Mhz
       Fosc= Fin*M/(N1*N2), Fcy=Fosc/2
       Fosc= 7992075Hz*(41)/(2*2)=79920750Hz for Fosc, Fcy = 39960375Hz (Crystal rate Measured on Test Board) */
    /* Configure PLL prescaler, PLL postscaler, PLL divisor */
    PLLFBD = 38; /* M = PLLFBD + 2 */
    CLKDIVbits.PLLPOST = 0; /* N1 = 2 */
    CLKDIVbits.PLLPRE = 0; /* N2 = 2 */
    
    __builtin_write_OSCCONH(0x03); /* New Oscillator selection FRC w/ PLL */
    __builtin_write_OSCCONL(0x01); /* Enable Switch */
    while (OSCCONbits.COSC != 0b011); /* Wait for Oscillator to switch to FRC w/ PLL */
    while (OSCCONbits.LOCK != 1); /* Wait for Pll to Lock */
    
    
   // __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 */




    InitIO();
 //   _LATA2 = 1;
 //   _LATF6 = 1;
     
   // while (1);
    
    InitVariables();
    InitPIDGains();

    //init TMR1 to interrupt at rate specified in config.h
    _T1IP = 5;
    TMR1 = 0; //Clear the timer
    //PR1 = 62438; //Should give a 10Hz interrupt at 39960375Hz clock
    PR1 = 6244; //Should give a 100Hz interrupt at 39960375Hz clock
    T1CON = 0x8020; //enabled, prescaler 1:64, internal clock
    _T1IF = 0; //Clear Interrupt
    _T1IE = 0; //Keep Interrupt disabled until everything is set up.
    // _T1IP = ;
    T1_Freq = (unsigned int) (((long) 39960375) / 64 / PR1);  //cts/second
    
    

    //init TMR5 to provide ~1kHz interuput 
    T5CON = 0x0010; //Start with timer disabled, prescaler = 8
    PR5 = 1245; //~4012kHz  with Clock at  39960375MHz and prescaler = 1:8. 
    //PR5 = 4574; //~1092Hz with Clock at  39960375MHz and prescaler = 1:8. 
    //PR5 = 2 * 4574; //~512Hz with Clock at  39960375MHz and prescaler = 1:8. 
    _T5IF = 0; //Clear interrupt
    _T5IP = 5;
    _T5IE = 1; //Enable interrupt.
    T5CON = 0x8010;
    T5_Freq = (unsigned int) (((long) 39960375) / 8 / PR5);
    
    // init TMR3 to provide the timebase for interrupt that starts the AIO process
    //T3CON = 0x8000;
    T3CON = 0x0000; //Start with timer disabled
    PR3 = 220; //Rolls over about every 6uSec and initiates ADC sampling.  This timing needs to be compared to ADC sample and hold times...
    _T3IP = 5;
    _T3IF = 0; //Clear interrupt
    _T3IE = 1; //Enable interrupt.

       
    
    //Set up UARTS
#ifdef DEBUG
    InitU1(); //RS-232
#endif
    InitU2();
    _U1RXIP = 6;

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

    InitSPI();

    PORTA = 0;

    // Enable Timer interrupts now that everything is set up.
    AD1CON1bits.ADON = 1; //Enable ADC, this starts sampling and thus creates DMA interrupt from which most everything happens.
    _T1IE = 1; //Enable 25Hz Interrupt, _T1Interrupt gets called at the beginning of each period.

    

// display startup banner
 //   __delay_ms(1000);
  //  putsU2("4K Camera BottomSide Controller\r\n");
  //  sprintf(s, "Built on %s at %s \r\n", __DATE__, __TIME__);
 //   putsU2(s);
 //   __delay_ms(1000);

    // init the i2c bus
    i2c_init();

    // storage for converted values
    float hum_val = 0.0;
    float temp_val = 0.0;
    float press_val = 0.0;

    // mpl115a2 calibration coefficients
    float a0, b1, b2, c12;

    i2c_getMPL115A2Coeff(&a0, &b1, &b2, &c12);

  //  putsU2("**************************\r\n");
  //  putsU2("MPL115A2 COEFFICIENTS\r\n");

 //   sprintf(s, "a0  = %.6f \r\n", a0);
  //  putsU2(s);

  //  sprintf(s, "b1  = %.6f \r\n", b1);
 //   putsU2(s);

  //  sprintf(s, "b2  = %.6f \r\n", b2);
 //   putsU2(s);

 //   sprintf(s, "c12 = %.6f \r\n", c12);
 //   putsU2(s);

  //  __delay_ms(2500);
    
    
   float  update_time = f_time;




    
    while (1) {
#ifdef DEBUG
        
        
        if (f_time > update_time) //Wait for time to be updated.
        {
            update_time += 1.0;

            /////////////////////////////////////////////////////////////////
            // get temp and humidity

            i2c_getHumTemp(&hum_val, &temp_val);

            /////////////////////////////////////////////////////////////////
            // display results

         //   putsU2("**************************\r\n");
         //   sprintf(s, "TEMP1   : %.2f C\r\n", temp_val);
         //   putsU2(s);

          //  sprintf(s, "HUM     : %.2f %%\r\n", hum_val);
         //   putsU2(s);

          //  putsU2("\r\n");

            /////////////////////////////////////////////////////////////////
            // get temp and pressure

            i2c_getPressTemp(a0, b1, b2, c12, &press_val, &temp_val);

            /////////////////////////////////////////////////////////////////
            // display results

        //    sprintf(s, "TEMP2   : %.2f C\r\n", temp_val);
       //     putsU2(s);

        //    sprintf(s, "PRESSURE: %.2f kPa\r\n", press_val);
        //    putsU2(s);

        //    putsU2("\r\n");

       //     sprintf(s, "UPTIME  : %.2f SECONDS\r\n", (f_time/10.0));
        //    putsU2(s);
            
            EnterCriticalSection();   
            SerialDataOut.Humidity = hum_val;   
            ExitCriticalSection();
            EnterCriticalSection();   
            SerialDataOut.Pressure = press_val;  
            ExitCriticalSection();
 

        }
        
        
        if (f_time > last_time) //Wait for time to be updated.
        {    
        
            
        last_time = f_time;
  


      
#if defined(DEBUG) && defined(ALLOW_CALIBRATION)

            if (raw == 1)
            {
                sprintf(s, "%f     %d     %d     %d       %d\r\n", f_time,
                    PhaseA_Curr,PhaseB_Curr,VBus_ADC[0], IBatt_ADC[0]
                    );
            }
            else
#endif
            {
                
                        sprintf(s, "%.1f  |  FocusTarg =  %6d    ZoomRateTarg =  %6d    FocusFollow = %d  %.3f  ZoomFollow = %d  %.3f |  Camera Temp = %6d  %.2f   Fwd H20 = %1d %1d   Aft H20 = %1d %1d   LSR_PWR_REQ =  %d  _RD5 = %d\r\n",
                        f_time, 
                                SerialDataIn->FocusTarget,SerialDataIn->ZoomRateTarget,  
                                FocusFollow,ConvertIntToEngUnits(FocusFollow, FocusFollow_cnt, FocusFollow_eng),
                                ZoomFollow, SerialDataOut.ZoomFollow = ConvertIntToEngUnits(ZoomFollow, ZoomFollow_cnt, ZoomFollow_eng),
                                SerialDataOut.CameraTemp,ConvertIntToEngUnits(CameraTemp, ADC_CameraTemp_cnt, ADC_CameraTemp_eng),
                                FWD_H20_ALARM_ACTIVELOW,bottom_status.bitfields.FwdWaterAlarm,AFT_H20_ALARM_ACTIVELOW,bottom_status.bitfields.AftWaterAlarm,SerialDataIn->status.bitfields.LaserPowerRequest,_RD5
                                ); 
                
            } 
            putsU2(s);
                                 
            
          

        }
#endif
    }
    return 0;
} //main