// ----------------------------------------------------------------------------------------
//
//  common/gps/nmea.cpp
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
//    
//  This file implements a state machine to parse selected NMEA sentences.
//
//  Copyright (C) 2008 Bob Cook
//
//      This program is free software; you can redistribute it and/or modify it
//      under the terms of the GNU General Public License as published by the
//      Free Software Foundation; either version 2 of the License, or (at your
//      option) any later version.
//    
//      This program is distributed in the hope that it will be useful, but 
//      WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
//      or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License 
//      for more details.
//    
//      You should have received a copy of the GNU General Public License along
//      with this program; if not, write to the Free Software Foundation, Inc., 
//      51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
//
//
//  A useful site for NMEA formats:
//
//      http://www.gpsinformation.org/dale/nmea.htm
//
// ----------------------------------------------------------------------------------------


#include "nmea.h"

#include <string.h>

#include "packages/common/util/bcdpacker.h"
#include "packages/common/util/misc.h"

#include "project_defs.h"


// ----------------------------------------------------------------------------------------

#if !defined( PRJ_FIXED16DOT16_ENABLE_CONVERT_FROM_STRING )
#error Support for converting strings to 16.16 fixed point numbers is required!
#endif


// ----------------------------------------------------------------------------------------

static uint8_t g_error_count;

static int16_t      g_latitude_deg;
static fixed16dot16 g_latitude_min;

static int16_t      g_longitude_deg;
static fixed16dot16 g_longitude_min;

static uint8_t g_enc_timestamp[ 6 ];
static uint8_t g_position_fix;
static uint8_t g_satellite_count;

const uint8_t state_start_of_sentence  =  0;  // wait for $
const uint8_t state_identify_command   =  1;  // wait for first ,
const uint8_t state_do_timestamp       =  2;  // UTC timestamp: hhmmss.sss
const uint8_t state_do_latitude_num    =  3;  // Latitude: ddmm.mmmm
const uint8_t state_do_latitude_dir    =  4;  // Latitude: N or S
const uint8_t state_do_longitude_num   =  5;  // Longitude: dddmm.mmmm
const uint8_t state_do_longitude_dir   =  6;  // Longitude: E or W
const uint8_t state_do_position_fix    =  7;  // Position Fix: 0, 1, 2, 3, etc.
const uint8_t state_do_satellite_count =  8;  // Satellites Used: 0 - 12
const uint8_t state_do_checksum        =  9;  // wait for *
const uint8_t state_do_checksum_1      = 10;  // first nybble of checksum
const uint8_t state_do_checksum_2      = 11;  // second nybble of checksum


// ----------------------------------------------------------------------------------------

static bool is_command_gpgga( uint8_t* buffer )
{
    if ( buffer[ 0 ] != 'G' )
    {
        return false;
    }

    if ( buffer[ 1 ] != 'P' )
    {
        return false;
    }

    if ( buffer[ 2 ] != 'G' )
    {
        return false;
    }

    if ( buffer[ 3 ] != 'G' )
    {
        return false;
    }

    if ( buffer[ 4 ] != 'A' )
    {
        return false;
    }

    return true;
}


// ----------------------------------------------------------------------------------------

static uint8_t convert_hex_char( uint8_t hexchar )
{
    if ( ( hexchar >= '0' ) && ( hexchar <= '9' ) )
    {
        return hexchar - static_cast< uint8_t >( '0' );
    }
    else if ( ( hexchar >= 'A' ) && ( hexchar <= 'F' ) )
    {
        return 10 + hexchar - static_cast< uint8_t >( 'A' );
    }
    else if ( ( hexchar >= 'a' ) && ( hexchar <= 'f' ) )
    {
        return 10 + hexchar - static_cast< uint8_t >( 'a' );
    }

    return 0;
}


// ----------------------------------------------------------------------------------------

static uint8_t convert_digit_char( uint8_t digitchar )
{
    if ( ( digitchar >= '0' ) && ( digitchar <= '9' ) )
    {
        return digitchar - static_cast< uint8_t >( '0' );
    }

    return 0;
}


// ----------------------------------------------------------------------------------------

static void convert_buffer_to_latitude( const uint8_t* buffer )
{
    g_latitude_deg = 0;
    g_latitude_min = 0;
    
    //--    Layout: DDMM.MMM

    if ( *buffer == '\0' )
    {
        return;
    }

    g_latitude_deg = convert_digit_char( *buffer++ );
     
    if ( *buffer == '\0' )
    {
        return;
    }

    g_latitude_deg *= 10;
    g_latitude_deg += convert_digit_char( *buffer++ );

    if ( *buffer == '\0' )
    {
        return;
    }

    g_latitude_min
        = fixed16dot16_from_string( reinterpret_cast< const char* >( buffer ) );
}


// ----------------------------------------------------------------------------------------

static void convert_buffer_to_longitude( const uint8_t* buffer )
{
    g_longitude_deg = 0;
    g_longitude_min = 0;
    
    //--    Layout: DDDMM.MMM

    if ( *buffer == '\0' )
    {
        return;
    }

    g_longitude_deg = convert_digit_char( *buffer++ );
     
    if ( *buffer == '\0' )
    {
        return;
    }

    g_longitude_deg *= 10;
    g_longitude_deg += convert_digit_char( *buffer++ );

    if ( *buffer == '\0' )
    {
        return;
    }

    g_longitude_deg *= 10;
    g_longitude_deg += convert_digit_char( *buffer++ );

    if ( *buffer == '\0' )
    {
        return;
    }

    g_longitude_min 
        = fixed16dot16_from_string( reinterpret_cast< const char* >( buffer ) );
}


// ----------------------------------------------------------------------------------------

bool nmea_parse( uint8_t c )
{
    static uint8_t buffer[ 16 ];
    static uint8_t bufsize;
    static uint8_t checksum;
    static uint8_t state = state_start_of_sentence;

    if ( c == '\n' || c == '\r' )
    {
        //--    End of line, reset to the start of the sentence.

        state = state_start_of_sentence;
        return false;
    }
    
    switch ( state )
    {
        case state_start_of_sentence:

            if ( c == '$' )
            {
                state = state_identify_command;
                bufsize = 0;
                checksum = 0;
            }

            break;

        case state_identify_command:
        case state_do_timestamp:
        case state_do_latitude_num:
        case state_do_longitude_num:

            checksum ^= c;

            if ( c == ',' )
            {
                if ( state == state_identify_command )
                {
                    if ( is_command_gpgga( buffer ) )
                    {
                        //--    First data field is the timestamp.

                        state = state_do_timestamp;
                        bufsize = 0;
                    }
                    else
                    {
                        //--    Not the command we want.

                        state = state_start_of_sentence;
                    }
                }
                else if ( state == state_do_timestamp )
                {
                    memset( g_enc_timestamp, 0, sizeof( g_enc_timestamp ) );
                    bcdpack_number_string( buffer, bufsize,
                                           g_enc_timestamp, sizeof( g_enc_timestamp ) );

                    //--    Next data field is the numerical value for latitude.

                    state = state_do_latitude_num;
                    bufsize = 0;
                }
                else if ( state == state_do_latitude_num )
                {
                    buffer[ bufsize++ ] = '\0';
                    convert_buffer_to_latitude( buffer );

                    //--    Next data field is the latitude direction.

                    state = state_do_latitude_dir;
                }
                else if ( state == state_do_longitude_num )
                {
                    buffer[ bufsize++ ] = '\0';
                    convert_buffer_to_longitude( buffer );

                    //--    Next data field is the longitude direction.

                    state = state_do_longitude_dir;
                }
                else
                {
                    //--    State machine error, start over at the next sentence.

                    state = state_start_of_sentence;
                    g_error_count++;
                }
            }
            else if ( bufsize >= array_sizeof( buffer ) )
            {
                //--    Error, buffer overrun.

                state = state_start_of_sentence;
                g_error_count++;
            }
            else
            {
                buffer[ bufsize++ ] = c;
            }

            break;

        case state_do_latitude_dir:

            checksum ^= c;

            if ( c == ',' )
            {
                // next data field is the numerical value for longitude
                state = state_do_longitude_num;
                bufsize = 0;
            }
            else if ( c == 'S' )
            {
                g_latitude_deg *= -1;
            }

            break;

        case state_do_longitude_dir:

            checksum ^= c;

            if ( c == ',' )
            {
                // next data field is the position fix indicator
                state = state_do_position_fix;
                g_position_fix = 0; // by default, we don't have a fix yet
            }
            else if ( c == 'W' )
            {
                g_longitude_deg *= -1;
            }

            break;

        case state_do_position_fix:

            checksum ^= c;

            if ( c == ',' )
            {
                // next data field is the satellite count
                state = state_do_satellite_count;
                g_satellite_count = 0;
            }
            else
            {
                g_position_fix = convert_hex_char( c );
            }

            break;

        case state_do_satellite_count:

            checksum ^= c;

            if ( c == ',' )
            {
                // skip straight to the checksum now
                state = state_do_checksum;
            }
            else
            {
                g_satellite_count *= 10;
                g_satellite_count += c - '0';
            }

            break;

        case state_do_checksum:

            if ( c == '*' )
            {
                state = state_do_checksum_1;
                // bufsize holds the input sentence's checksum
                bufsize = 0;
            }
            else
            {
                checksum ^= c;
            }

            break;

        case state_do_checksum_1:

            // bufsize holds the input sentence's checksum
            bufsize = convert_hex_char( c );
            bufsize <<= 4;
            state = state_do_checksum_2;
            break;

        case state_do_checksum_2:

            // no matter what, next state is back to the start of the next sentence
            state = state_start_of_sentence;

            bufsize |= convert_hex_char( c );

            if ( checksum == bufsize )
            {
                // good sentence, return true
                return true;
            }

            break;

        default:

            // state machine error, start over at the next sentence
            state = state_start_of_sentence;
            break;
    }

    return false;
}


// ----------------------------------------------------------------------------------------

uint8_t nmea_error_count()
{
    return g_error_count;
}

    
// ----------------------------------------------------------------------------------------

void nmea_timestamp( uint8_t* timestamp )
{
    memcpy( timestamp, g_enc_timestamp, sizeof( g_enc_timestamp ) );
}


// ----------------------------------------------------------------------------------------

void nmea_latitude( int16_t* degrees, fixed16dot16* minutes )
{
    *degrees = g_latitude_deg;
    *minutes = g_latitude_min;
}


// ----------------------------------------------------------------------------------------

void nmea_longitude( int16_t* degrees, fixed16dot16* minutes )
{
    *degrees = g_longitude_deg;
    *minutes = g_longitude_min;
}


// ----------------------------------------------------------------------------------------

bool nmea_position_fix( uint8_t* satellite_count )
{
    if ( satellite_count )
    {
        *satellite_count = g_satellite_count;
    }
    return ( g_position_fix > 0 );
}

    
// ----------------------------------------------------------------------------------------
//  end of nmea.cpp
// ----------------------------------------------------------------------------------------

