/*
 * 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 "navproc_defs.h"
#include "../CommonCode/crit_section.h"

/* command parser */
#include "ts_actions.h"
#include "../CommonCode/parser.h"

/* i2c periperals */
#include "../CommonCode/i2c.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

/* pan tilt button funcs */
void checkPtFineControl();
unsigned char getNpButtons();
void setNpLeds(unsigned char state);
int scaleJoyStick(fractional val);

/* get commands from UART2 */
int getCommand(char* cmd);

/* Andy's debug string of mystery */
void formatDebugString(char* buff);

int main(void)
{
    /* CLI variables */
    int cmd_rc;
    char* cmd_ptr;
    char cmd_buff[128];
    char msg_buff[512];

    /* time keeping variables */
    double now_tm = 0.0;
    double ptb_update_tm = 0.0;
    double cam_update_tm = 0.0;
    double env_update_tm = 0.0;
    double aho_update_tm = 0.0;

    /* joystick variables */
    int joy_x_val = 0;
    int joy_y_val = 0;

    fractional joy_x_raw = 0;
    fractional joy_y_raw = 0;

    /* camera variables */
    double camera_zoom = 0;
    double camera_focus = 0;
    double camera_iris = 0;

    /* environmental variables */
    double camera_temp = 0.0;
    double camera_hum = 0.0;
    double camera_press = 0.0;
    int camera_leak = 0;

    /* 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();

    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 = ;


    //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;

    // 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.

    /* init the i2c bus */
    i2c_init();

    /* display startup banner */
    __delay_ms(1000);
    u2PutString("4K Camera TopSide Controller\r\n");
    sprintf(msg_buff, "Built on %s at %s \r\n", __DATE__, __TIME__);
    u2PutString(msg_buff);
    __delay_ms(1500);

    /* initialize update timers with now time */
    EnterCriticalSection();
    now_tm = f_time;
    ExitCriticalSection();

    ptb_update_tm = now_tm;
    cam_update_tm = now_tm;
    env_update_tm = now_tm;
    aho_update_tm = now_tm;
  
    
    for (;;)
    {
        EnterCriticalSection();
        now_tm = f_time;
        ExitCriticalSection();

        /* send lapbox pan and tilt interface string */
        if ( (now_tm > ptb_update_tm) && ptbOutput )
        {
            /* update the timer */
            ptb_update_tm += 0.05;

            EnterCriticalSection();
            joy_x_raw = JoyStk_X;
            joy_y_raw = JoyStk_Y;
            ExitCriticalSection();

            joy_x_val = scaleJoyStick(joy_x_raw);
            joy_y_val = scaleJoyStick(joy_y_raw);

            if ( JOYSTCK_SEN_LED )
            {
                joy_x_val /= 2;
                joy_y_val /= 2;

                joy_x_val += 25;
                joy_y_val += 25;
            }

            sprintf(msg_buff, "PTB, %d, %d, %02x\r\n",
                    joy_x_val, joy_y_val, getNpButtons());

            u2PutString(msg_buff);
            
               
       //     sprintf(msg_buff, "Focus   %d  %d \r\n",
       //             Focus_Slider,
       //             SerialDataOut.FocusTarget);
       //     sprintf(msg_buff, "Focus Targ =  %d  Focus Follow =  %g V (0V = -32768, 5V = 32767)\r\n",
       //             SerialDataOut.FocusTarget,
       //             SerialDataIn->FocusFollow);
       //      sprintf(msg_buff, "Laser Request =  %d  Laser State =  %d\r\n",
       //             SerialDataOut.status.bitfields.LaserPowerRequest,
       //             SerialDataIn->status.bitfields.LaserPowerState);
       //     u2PutString(msg_buff);
            
        }

        /* send camera data string */
        if ( (now_tm > cam_update_tm) && camOutput )
        {
            /* update the timer */
            cam_update_tm += 0.5;

            EnterCriticalSection();
            camera_zoom = SerialDataIn->ZoomFollow;
            camera_focus = SerialDataIn->FocusFollow;
            camera_iris = 0;
            ExitCriticalSection();

            sprintf(msg_buff, "CAM, %.3f, %.3f, %.3f\r\n",
                    camera_zoom, camera_focus, camera_iris);

            u2PutString(msg_buff);
        }

        /* send camera enviro data string */
        if ( (now_tm > env_update_tm) && envOutput )
        {
            /* update the timer */
            env_update_tm += 1.0;

            EnterCriticalSection();
            camera_hum = SerialDataIn->Humidity;
            camera_temp = ((float) SerialDataIn->CameraTemp)/10;
            camera_press = SerialDataIn->Pressure;
            camera_leak = (SerialDataIn->status.bitfields.AftWaterAlarm) ||
                          (SerialDataIn->status.bitfields.FwdWaterAlarm);
            ExitCriticalSection();

            sprintf(msg_buff, "ENV, %.2f, %.2f, %.2f, %d\r\n",
                    camera_temp, camera_hum, camera_press, camera_leak);
            u2PutString(msg_buff);
            
            
            
        }

        /* if any timers that have fallen behind, fast forward them */
        if ( now_tm > ptb_update_tm ) ptb_update_tm = now_tm + 0.05;
        if ( now_tm > cam_update_tm ) cam_update_tm = now_tm + 0.50;
        if ( now_tm > env_update_tm ) env_update_tm = now_tm + 1.00;

        /* update the led state based on the LED_OUT command */
        setNpLeds(npLedState);

        /* toggle "P&T Fine Control" LED if the button was pressed */
        checkPtFineControl();

        /* get command */
        if ( getCommand(cmd_buff) )
        {
            /* clear return code */
            cmd_rc = 0;

            /* skip white space here */
            cmd_ptr = prsSkip(cmd_buff, " \t\n");

            /* if the cmd from getCommand was empty then return here. */
            if (strlen(cmd_ptr))
                cmd_rc = prsParse(cmd_ptr, cmdsRoot, ERR_NO_COMMAND);

            if ((cmd_rc == ERR_NO_COMMAND) && echoMode)
                u2PutString("ERR: UNKNOWN COMMAND\r\n");

            if ((cmd_rc == ERR_NO_SUB_COMMAND) && echoMode)
                u2PutString("ERR: UNKNOWN SUB COMMAND\r\n");
        }

        if ( tsDebugOutput )
        {
            if ( now_tm > aho_update_tm )
            {
                aho_update_tm += 0.1;

                formatDebugString(msg_buff);
                putsU2(msg_buff);
            }

            // catch up the Andy H. output timer if it's fallen behind
            if ( aho_update_tm < now_tm ) aho_update_tm = now_tm + 0.1;
        }
    }
    return 0;

} //main

void checkPtFineControl()
{
    int pt_fine = 0;
    static int old_pt_fine = 0;

    if ( JOYSTCK_SEN_SW )
        pt_fine = 1;

    /* toggle "P&T Fine Control" on a zero to one transition */
    if ( !old_pt_fine && pt_fine )
        JOYSTCK_SEN_LED = !JOYSTCK_SEN_LED;

    old_pt_fine = pt_fine;
}

unsigned char getNpButtons()
{
    unsigned char b = 0x00;

    if ( JOYSTCK_SEN_SW ) b |= BTN_EMPTY_1;
    if ( JOYSTCK_REV_SW ) b |= BTN_ENABLE;
    if ( PORT_LIGHT_SW )  b |= BTN_PORT_LT;
    if ( PORT_CAM_SW )    b |= BTN_PORT_PT;
    if ( STBD_CAM_SW )    b |= BTN_STBD_PT;
    if ( STBD_LIGHT_SW )  b |= BTN_STBD_LT;

    return b;
}

void setNpLeds(unsigned char state)
{
/*
    note: this LED is handled in checkPtFineControl()
    JOYSTCK_SEN_LED = (( state & LED_EMPTY_1 ) ? 1 : 0);
*/

    JOYSTCK_REV_LED = (( state & LED_ENABLE  ) ? 1 : 0);
    PORT_LIGHT_LED  = (( state & LED_PORT_LT ) ? 1 : 0);
    PORT_CAM_LED    = (( state & LED_PORT_PT ) ? 1 : 0);
    STBD_CAM_LED    = (( state & LED_STBD_PT ) ? 1 : 0);
    STBD_LIGHT_LED  = (( state & LED_STBD_LT ) ? 1 : 0);

    /* Alternatively, if dimming is needed, set the bits in the top_status structure here and let ManageLEDs set them ...
    JOYSTCK_REV_LED = top_status.bitfields.JoystickReversalRequest;// & pwm_state;
    JOYSTCK_SEN_LED = top_status.bitfields.JoystickSensitivityRequest;// & pwm_state;
    PORT_LIGHT_LED = top_status.bitfields.PortLightRequest;// & pwm_state;
    STBD_LIGHT_LED = top_status.bitfields.StbdLightRequest;// & pwm_state;
    PORT_CAM_LED = top_status.bitfields.PortCamRequest;// & pwm_state;
    STBD_CAM_LED = top_status.bitfields.StbdCamRequest;// & pwm_state;
*/
    return;
}

int scaleJoyStick(fractional val)
{
    val /= 327;
    if ( val > 100 ) val = 100;
    if ( val < -100 ) val = -100;

    return val;
}

#define MAX_CHARS   80
int getCommand(char* cmd)
{
    static int char_count = 0;
    static char line[MAX_CHARS] = "";
    const unsigned char terminator = '\r';
    int got_line;
    unsigned char b;
    int i;

    got_line = 0;

    if ( u2GetByte(&b) )
    {
        if ( echoMode )
        {
            u2PutByte(b);
            if (b == '\r') u2PutByte('\n');
        }

        if ( b != terminator )
        {
            if ( (b != '\b') && (char_count < MAX_CHARS) )
                line[char_count++] = b;
            else if ( char_count > 0 )
                --char_count;

            cmd[0] = '\0';
        }
        else
        {
            got_line = 1;
            line[char_count] = '\0';
            for (i = 0; i <= char_count; i++)
                cmd[i] = line[i];

            char_count = 0;
        }
    }

    return got_line;
}

void formatDebugString(char* buff)
{
            sprintf(buff, "%.3f   OverTempState: %d    WaterAlms: %d  %d       CameraTemp: %.1f        ZoomFollow: %.3f   FocusFollow: %.3f           CanHum: %.1f     CanPress: %.1f  JoyX = %d  JoyY = %d   Zoom Rcker:  %d   %d      Focus:  %d   %d\r\n",
                    f_time,
                    SerialDataIn->status.bitfields.OverTemperatureState,
                    SerialDataIn->status.bitfields.FwdWaterAlarm, SerialDataIn->status.bitfields.AftWaterAlarm,
                    ((float) SerialDataIn->CameraTemp)/10,
                    SerialDataIn->ZoomFollow,
                    SerialDataIn->FocusFollow,
                    SerialDataIn->Humidity,
                    SerialDataIn->Pressure,
                    JoyStk_X,JoyStk_Y,
                    iTemp1,SerialDataOut.ZoomRateTarget,
                                        iTemp2,SerialDataOut.FocusTarget
                    );

    return;
}