#include "mbed.h"
#include "TFC.h"

 
//This macro is to maintain compatibility with Codewarrior version of the sample.   This version uses the MBED libraries for serial port access
Serial PC(USBTX,USBRX);

#define TERMINAL_PRINTF     PC.printf

 
 //This ticker code is used to maintain compability with the Codewarrior version of the sample.   This code uses an MBED Ticker for background timing.
 
#define NUM_TFC_TICKERS 4

Ticker TFC_TickerObj;
 
volatile uint32_t TFC_Ticker[NUM_TFC_TICKERS];
 
void TFC_TickerUpdate()
{
    int i;
 
    for(i=0; i<NUM_TFC_TICKERS; i++)
     {
        if(TFC_Ticker[i]<0xFFFFFFFF) 
        {
            TFC_Ticker[i]++;
        }
    }
}
 
 
int main()
{
    uint32_t i,t = 0;
  
    PC.baud(115200);
    TFC_TickerObj.attach_us(&TFC_TickerUpdate,2000);
   
    TFC_Init();
    
    for(;;)
    {      
        //TFC_Task must be called in your main loop.  This keeps certain processing happy (I.E. Serial port queue check)
         //   TFC_Task();

            //This Demo program will look at the middle 2 switch to select one of 4 demo modes.
            //Let's look at the middle 2 switches
            switch((TFC_GetDIP_Switch()>>1)&0x03)
            {
            default:
            case 0 :
                //Demo mode 0 just tests the switches and LED's
                if(TFC_PUSH_BUTTON_0_PRESSED)
                    TFC_BAT_LED0_ON;
                else
                    TFC_BAT_LED0_OFF;
                
                if(TFC_PUSH_BUTTON_1_PRESSED)
                    TFC_BAT_LED3_ON;
                else
                    TFC_BAT_LED3_OFF;
                
                
                if(TFC_GetDIP_Switch()&0x01)
                    TFC_BAT_LED1_ON;
                else
                    TFC_BAT_LED1_OFF;
                
                if(TFC_GetDIP_Switch()&0x08)
                    TFC_BAT_LED2_ON;
                else
                    TFC_BAT_LED2_OFF;
                
                break;
                    
            case 1:
                
                //Demo mode 1 will just move the servos with the on-board potentiometers
                if(TFC_Ticker[0]>=20)
                {
                    TFC_Ticker[0] = 0; //reset the Ticker
                    //Every 20 mSeconds, update the Servos
                    TFC_SetServo(0,TFC_ReadPot(0));
                    TFC_SetServo(1,TFC_ReadPot(1));
                }
                //Let's put a pattern on the LEDs
                if(TFC_Ticker[1] >= 125)
                {
                    TFC_Ticker[1] = 0;
                    t++;
                    if(t>4)
                    {
                        t=0;
                    }           
                    TFC_SetBatteryLED_Level(t);
                }
                
                TFC_SetMotorPWM(0,0); //Make sure motors are off
                TFC_HBRIDGE_DISABLE;
            

                break;
                
            case 2 :
                
                //Demo Mode 2 will use the Pots to make the motors move
                TFC_HBRIDGE_ENABLE;
               
                TFC_SetMotorPWM(TFC_ReadPot(0),TFC_ReadPot(1));
                
                        
                //Let's put a pattern on the LEDs
                if(TFC_Ticker[1] >= 125)
                    {
                        TFC_Ticker[1] = 0;
                            t++;
                            if(t>4)
                            {
                                t=0;
                            }           
                        TFC_SetBatteryLED_Level(t);
                    }
                break;
            
            case 3 :
            
         
                //Demo Mode 3 will be in Freescale Garage Mode.  It will beam data from the Camera to the 
                //Labview Application
                
                
                if(TFC_Ticker[0]>50 && TFC_LineScanImageReady>0)
                    {
                     TFC_Ticker[0] = 0;
                     TFC_LineScanImageReady=0;
                     TERMINAL_PRINTF("\r\n");
                     TERMINAL_PRINTF("L:");
                     
                        if(t==0)
                            t=4;
                        else
                            t--;
                        
                         TFC_SetBatteryLED_Level(t);
                        
                         // camera 1
                         for(i=0;i<128;i++)
                         {
                               TERMINAL_PRINTF("%X,",TFC_LineScanImage0[i]);
                         }
                        
                        // camera 2
                         for(i=0;i<128;i++)
                         {
                                 if(i==127)
                                     TERMINAL_PRINTF("%X\r\n",TFC_LineScanImage1[i]);
                                 else
                                     TERMINAL_PRINTF("%X,",TFC_LineScanImage1[i]);
                           
                         }
                    }
                 
                break;
            }
    }
    
 
}