#include "Header_Files/sbglogparse.h"

using namespace sbgtc;

SbgErrorCode sbgtc::sbgEComBinaryLogParseEkfEulerData(SbgStreamBuffer *pInputStream, SbgLogEkfEulerData *pOutputData)
{
    //assert(pInputStream);
    //assert(pOutputData);

    //
    // Read the frame payload
    //
    pOutputData->timeStamp		= sbgStreamBufferReadUint32LE(pInputStream);

    pOutputData->euler[0]		= sbgStreamBufferReadFloatLE(pInputStream);
    pOutputData->euler[1]		= sbgStreamBufferReadFloatLE(pInputStream);
    pOutputData->euler[2]		= sbgStreamBufferReadFloatLE(pInputStream);

    pOutputData->eulerStdDev[0]	= sbgStreamBufferReadFloatLE(pInputStream);
    pOutputData->eulerStdDev[1]	= sbgStreamBufferReadFloatLE(pInputStream);
    pOutputData->eulerStdDev[2]	= sbgStreamBufferReadFloatLE(pInputStream);

    pOutputData->status			= sbgStreamBufferReadUint32LE(pInputStream);

    //
    // Return if any error has occurred while parsing the frame
    //
    return sbgStreamBufferGetLastError(pInputStream);
}

SbgErrorCode sbgtc::sbgEComBinaryLogParseUtcData(SbgStreamBuffer *pInputStream, SbgLogUtcData *pOutputData)
{
//    assert(pInputStream);
//    assert(pOutputData);

    //
    // Read the frame payload
    //
    pOutputData->timeStamp		= sbgStreamBufferReadUint32LE(pInputStream);
    pOutputData->status			= sbgStreamBufferReadUint16LE(pInputStream);
    pOutputData->year			= sbgStreamBufferReadUint16LE(pInputStream);
    pOutputData->month			= sbgStreamBufferReadInt8LE(pInputStream);
    pOutputData->day			= sbgStreamBufferReadInt8LE(pInputStream);
    pOutputData->hour			= sbgStreamBufferReadInt8LE(pInputStream);
    pOutputData->minute			= sbgStreamBufferReadInt8LE(pInputStream);
    pOutputData->second			= sbgStreamBufferReadInt8LE(pInputStream);
    pOutputData->nanoSecond		= sbgStreamBufferReadInt32LE(pInputStream);
    pOutputData->gpsTimeOfWeek	= sbgStreamBufferReadUint32LE(pInputStream);

    //
    // Return if any error has occurred while parsing the frame
    //
    return sbgStreamBufferGetLastError(pInputStream);
}



/*!
 * \param[in]	pInputStream				Input stream buffer to read the payload from.
 * \param[out]	pOutputData					Pointer on the output structure that stores parsed data.
 * \return									SBG_NO_ERROR if the payload has been parsed.
 */
SbgErrorCode sbgtc::sbgEComBinaryLogParseGpsPosData(SbgStreamBuffer *pInputStream, SbgLogGpsPos *pOutputData)
{

    //
    // Read the frame payload
    //
    pOutputData->timeStamp			= sbgStreamBufferReadUint32LE(pInputStream);
    pOutputData->status				= sbgStreamBufferReadUint32LE(pInputStream);
    pOutputData->timeOfWeek			= sbgStreamBufferReadUint32LE(pInputStream);
    pOutputData->latitude			= sbgStreamBufferReadDoubleLE(pInputStream);
    pOutputData->longitude			= sbgStreamBufferReadDoubleLE(pInputStream);
    pOutputData->altitude			= sbgStreamBufferReadDoubleLE(pInputStream);
    pOutputData->undulation			= sbgStreamBufferReadFloatLE(pInputStream);
    pOutputData->latitudeAccuracy	= sbgStreamBufferReadFloatLE(pInputStream);
    pOutputData->longitudeAccuracy	= sbgStreamBufferReadFloatLE(pInputStream);
    pOutputData->altitudeAccuracy	= sbgStreamBufferReadFloatLE(pInputStream);

    //
    // Test if we have a additional information such as base station id (since version 1.4)
    //
    if (sbgStreamBufferGetSpace(pInputStream) >= 5)
    {
        //
        // Read the additional information
        //
        pOutputData->numSvUsed			= sbgStreamBufferReadUint8LE(pInputStream);
        pOutputData->baseStationId		= sbgStreamBufferReadUint16LE(pInputStream);
        pOutputData->differentialAge	= sbgStreamBufferReadUint16LE(pInputStream);
    }
    else
    {
        //
        // Default the additional information
        //
        pOutputData->numSvUsed = 0;
        pOutputData->baseStationId = 0xFFFF;
        pOutputData->differentialAge = 0xFFFF;
    }

    //
    // Return if any error has occurred while parsing the frame
    //
    return sbgStreamBufferGetLastError(pInputStream);
}