// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/*
        GPS_MTK.cpp - Ublox GPS library for Arduino
        Code by Jordi Mu�oz and Jose Julio. DIYDrones.com
        This code works with boards based on ATMega168/328 and ATMega1280 (Serial port 1)

        This library is free software; you can redistribute it and/or
                modify it under the terms of the GNU Lesser General Public
                License as published by the Free Software Foundation; either
                version 2.1 of the License, or (at your option) any later version.

        GPS configuration : Costum protocol
        Baud rate : 38400

        Methods:
                init() : GPS initialization
                update() : Call this funcion as often as you want to ensure you read the incomming gps data
                
        Properties:
                lattitude : lattitude * 10000000 (long value)
                longitude : longitude * 10000000 (long value)
                altitude :      altitude * 100 (meters) (long value)
                ground_speed : Speed (m/s) * 100 (long value)
                ground_course : Course (degrees) * 100 (long value)
                new_data : 1 when a new data is received.
                                                        You need to write a 0 to new_data when you read the data
                fix : 1: GPS NO fix, 2: 2D fix, 3: 3D fix.
                        
*/
#include "AP_GPS_IMU.h"
#include "WProgram.h"


// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_IMU::AP_GPS_IMU(Stream *s) : GPS(s)
{
}


// Public Methods //////////////////////////////////////////////////////////////
void 
AP_GPS_IMU::init(void)
{
	// we expect the stream to already be open at the corret bitrate
	idleTimeout = 1200;
}

// optimization : This code doesn't wait for data. It only proccess the data available.
// We can call this function on the main loop (50Hz loop)
// If we get a complete packet this function calls parse_IMU_gps() to parse and update the GPS info.
bool 
AP_GPS_IMU::read(void)
{
        byte data;
        int numc = 0;

        numc = _port->available();

        if (numc > 0){
                for (int i=0;i<numc;i++){       // Process bytes received               

                        data = _port->read();
                        
                        switch(step){            //Normally we start from zero. This is a state machine
                                case 0: 
                                        if(data == 0x44)        // IMU sync char 1
                                                step++;  //OH first data packet is correct, so jump to the next step
                                        break; 
                                        
                                case 1: 
                                        if(data == 0x49)        // IMU sync char 2
                                                step++;  //ooh! The second data packet is correct, jump to the step 2
                                        else 
                                                step=0;  //Nop, is not correct so restart to step zero and try again.            
                                        break;
        
                                case 2: 
                                        if(data == 0x59)        // IMU sync char 3
                                                step++;  //ooh! The second data packet is correct, jump to the step 2
                                        else 
                                                step=0;  //Nop, is not correct so restart to step zero and try again.            
                                        break;
        
                                case 3: 
                                        if(data == 0x64)        // IMU sync char 4
                                                step++;  //ooh! The second data packet is correct, jump to the step 2
                                        else 
                                                step=0;  //Nop, is not correct so restart to step zero and try again.            
                                        break;
                                                                                
                                case 4:
                                        payload_length = data;
                                        checksum(payload_length);
                                        step++;
                                        if (payload_length > 28){
                                                step = 0;        //Bad data, so restart to step zero and try again.              
                                                payload_counter = 0;
                                                ck_a = 0;
                                                ck_b = 0;
                                                //payload_error_count++;
                                        } 
                                        break;
                                        
                                case 5:
                                        message_num = data;
                                        checksum(data);
                                        step++;
                                        break;
        
                                case 6: // Payload data read...
                                        // We stay in this state until we reach the payload_length
                                        buffer[payload_counter] = data;
                                        checksum(data);
                                        payload_counter++;
                                        if (payload_counter >= payload_length) { 
                                                step++; 
                                        }
                                        break;
                                        
                                case 7:
                                        GPS_ck_a = data;         // First checksum byte
                                        step++;
                                        break;
                                                                                
                                case 8:
                                        GPS_ck_b = data;         // Second checksum byte
        
                                        // We end the IMU/GPS read...
                                        // Verify the received checksum with the generated checksum.. 
                                        if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)) {
                                                if (message_num == 0x02) {
                                                        join_data();
                                                } else if (message_num == 0x03) {
                                                        GPS_join_data();
                                                } else if (message_num == 0x04) {
                                                        join_data_xplane();
                                                } else if (message_num == 0x0a) {
                                                        //PERF_join_data();
                                                } else {
//                                                      _error("Invalid message number = %d\n", (int)message_num);
                                                }
                                        } else {
//                                              _error("XXX Checksum error\n"); //bad checksum
                                                //imu_checksum_error_count++;
                                        }                                                
                                        // Variable initialization
                                        step = 0;
                                        payload_counter = 0;
                                        ck_a = 0;
                                        ck_b = 0;
                                        break;
                        }
                }// End for...
        }
        return true;
}

/****************************************************************
 * 
 ****************************************************************/

void AP_GPS_IMU::join_data(void)
{
        //Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other IMU classes.. 
        //In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet.

         //Storing IMU roll
        roll_sensor = *(int *)&buffer[0];

         //Storing IMU pitch
        pitch_sensor = *(int *)&buffer[2];

         //Storing IMU heading (yaw)
        ground_course = *(int *)&buffer[4];
        imu_ok = true;
}

void AP_GPS_IMU::join_data_xplane()
{
         //Storing IMU roll
        roll_sensor = *(int *)&buffer[0];
        

         //Storing IMU pitch
        pitch_sensor = *(int *)&buffer[2];

         //Storing IMU heading (yaw)
        ground_course = *(unsigned int *)&buffer[4];
        
         //Storing airspeed
        airspeed = *(int *)&buffer[6];

        imu_ok = true;

}

void AP_GPS_IMU::GPS_join_data(void)
{
        longitude = *(long *)&buffer[0];        // degrees * 10e7
        latitude  = *(long *)&buffer[4];

        //Storing GPS Height above the sea level
        altitude = (long)*(int *)&buffer[8] * 10;

        //Storing Speed
        speed_3d = ground_speed = (float)*(int *)&buffer[10];
        
        //We skip the gps ground course because we use yaw value from the IMU for ground course
        time = *(long *)&buffer[14];

        imu_health = buffer[15];
        
        new_data        = true;
        fix             = true;
}



/****************************************************************
 * 
 ****************************************************************/
// checksum algorithm
void AP_GPS_IMU::checksum(byte data)
{
        ck_a += data;
        ck_b += ck_a; 
}


/****************************************************************
 * Unused
 ****************************************************************/
void AP_GPS_IMU::setHIL(long _time, float _latitude, float _longitude, float _altitude,
            float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) {};