//i2c_Read_SPI_Data_Task.c//  //This runs on the MAIN processor//
#include <__cross_studio_io.h>
#include <salvo.h>
#include "salvocfg.h"
#include "i2c_Master.h"
#include "global_typedefs.h"
#include "gps_driver_MainP.h"
//#define COMMAND_GET_GPS_NAV_DATA   0x94          
//#define COMMAND_GET_GPS_UTC_TIME   0x95   

extern uint_1 I2C_reading_data;   //defined externally in i2c_read_spi_data_task
static NavSolMsg_Type solution;            //GPS Data This has a blend of all the messages  //This will be important
NavTimeUTC_Type utc_time;           //GPS Data This will be my utc time.  I think
NavPosLLHMsg_Type pos_llh;          //This is the LLH data that actually matters
//sint_4 LLHH_ARRAY[4];                      //I'm going to pass this into my telemetry task

//uint_1 SlaveAddress = GPSAddress;  //Set Slave to be the GPS Board.  Don't really need this, I command the GPS directly

static uint_1 Data1 = COMMAND_GET_GPS_NAV_DATA;                 // Send command 1 to the slave 
static uint_2 command_length1 = sizeof(Data1);            
static uint_2 data_length1 = sizeof(solution);

static uint_1 Data2 = COMMAND_GET_GPS_UTC_TIME;                 // Send command 2 to the slave 
static uint_2 command_length2 = sizeof(Data2);                 
static uint_2 data_length2 = sizeof(utc_time);   

static uint_1 Data3 = COMMAND_GET_GPS_LLH_DATA;                 // Send command 3 to the slave 
static uint_2 command_length3 = sizeof(Data3);            
static uint_2 data_length3 = sizeof(pos_llh);

extern uint_1 i2c_mag_task_on;        //Used in Mode_Switching Task
extern uint_1 i2c_temp_task_on;        //Used in Mode_Switching Task
extern uint_1 i2c_current_task_on;        //Used in Mode_Switching Task
extern uint_1 i2c_nav_task_on;
extern uint_1 i2c_gps_utc_on;


void i2c_Read_GPS_Nav_Data_Task (void)
{
  while(1)
  {
  if(i2c_nav_task_on==1) //mode switching
  {       
      while (I2CDCTL&I2CBUSY);                  // Wait until I2C module has finished (Do I need this?)
      if(I2C_reading_data==0)
      {
      I2C_reading_data=1;                       //Monopolize the slave
      I2C_Init();                               //Initialize I2C... may need to change this to UART1
      
      //I decided I didn't want to read the solution anymore.  Just LLH.
      //I2C_Write( &Data1, command_length1, GPSAddress);   // Command the slave  NOTE:  &address of  *value at
      //OS_Delay(5);                        
      //I2C_Read( (uint_1 *)&solution, data_length1, GPSAddress); //Read solution data (a whole mess of stuff)
      //OS_Delay(5); 
      
      I2C_Write( &Data3, command_length3, GPSAddress);   // Command the slave  
      OS_Delay(5);                        
      I2C_Read( (uint_1 *)&pos_llh, data_length3, GPSAddress); //Read llh data (lat long alt)
      //debug_printf("itow %d\n",pos_llh.itow);               //uint32
      //debug_printf("Longitude %d\n",pos_llh.Longitude);     //int32
      //debug_printf("Latitude %d\n",pos_llh.Latitude);       //int32
      //debug_printf("Altitude %d\n",pos_llh.Altitude);       //int32
      //debug_printf("AltitudeSea %d\n",pos_llh.AltitudeSea); //int32
      //debug_printf("HorzAcc %d\n",pos_llh.HorzAcc);         //uint32
      //debug_printf("VertAcc %d\n",pos_llh.VertAcc);         //uint32
      //LLHH_ARRAY[0]=pos_llh.Longitude;
      //LLHH_ARRAY[1]=pos_llh.Latitude;
      //LLHH_ARRAY[2]=pos_llh.Altitude;
      //LLHH_ARRAY[3]=pos_llh.AltitudeSea;
     
      I2C_OFF();        //Turn I2C off, cuz it says so in the notes.
      I2C_reading_data=0;                      //Release the slave to other read tasks
      }
      OS_Delay (50);   //chill out for a bit
    } 
    else
    {
    OS_Delay(50);
    }
  } //End of while loop
}   //End of Task

void i2c_Read_GPS_UTC_Data_Task (void)
{

  while(1)
   {
   if(i2c_gps_utc_on==1)
   {       
   while (I2CDCTL&I2CBUSY);                     // Wait until I2C module has finished
   if(I2C_reading_data==0)
   {
   I2C_reading_data=1;                       //Monopolize the slave
   I2C_Init();                                  //Call I2C_Init   
   I2C_Write( &Data2, command_length2, GPSAddress);      // Address the slave
   //OS_Delay(5);                        
   I2C_Read( (uint_1 *)&utc_time, data_length2, GPSAddress);        //Read data
   //debug_printf("utc_time %d,%d,%d,%d,%d,%d\n",utc_time.Year,utc_time.Month,utc_time.Day,utc_time.Hour,utc_time.Min,utc_time.Sec);

   I2C_OFF();        //Turn I2C off, cuz it says so in the notes.
   I2C_reading_data=0;
   }
   OS_Delay (50);   //chill out for a bit
   }
   else
   {
    OS_Delay(50);
   }
  } //End of while loop
}   //End of Task