/*============================ INCLUDES ======================================*/

#include <stdint.h>
#include <string.h>
#include "constants.h"
#include "ublox.h"
#include "math.h"
#include "same_endian.h"

/*=============================== MARCOS =====================================*/


/*============================================================================*/

void UbxDecodeNavPvt(UbxStruct *sol, uint8_t frame[])
{
    uint8_t *p = frame + 6;
    uint8_t carrSoln, diffSoln;

    sol->towms     = GET_U32(p);
    sol->utc.year  = GET_U16(p + 4);
    sol->utc.month = GET_U08(p + 6);
    sol->utc.day   = GET_U08(p + 7);
    sol->utc.hour  = GET_U08(p + 8);
    sol->utc.min   = GET_U08(p + 9);
    sol->utc.isec  = GET_U08(p + 10);
    sol->utc.msec  = sol->towms % 1000;
    sol->TimeFlag  = GET_U08(p + 11); 
    
    sol->GpsType   = GET_U08(p + 20);
    sol->RtkType   = GET_U08(p + 21);
    sol->SvNum     = GET_U08(p + 23);
    sol->LonDeg    = GET_S32(p + 24);
    sol->LatDeg    = GET_S32(p + 28);
    sol->hgt       = GET_S32(p + 32);   // mm Height above ellipsoid
    sol->msl       = GET_S32(p + 36);   // mm Height above mean sea level
    sol->hacc      = GET_U32(p + 40); 
    sol->vacc      = GET_U32(p + 44);

    sol->vn        = GET_S32(p + 48); // mm/s
    sol->ve        = GET_S32(p + 52);
    sol->vd        = GET_S32(p + 56);
    
    sol->GroundSpeed        = GET_S32(p + 60) * B10_N3;
    sol->HeadingMotionDeg   = GET_S32(p + 64) * B10_N5;
    sol->sacc               = GET_U32(p + 68); 
    sol->cacc               = GET_U32(p + 72) * B10_N5;         
    sol->pdop               = GET_U16(p + 76) * B10_N2;
    sol->HeadingVehichleDeg = GET_S32(p + 84) * B10_N5;

    sol->DateValid          = GET_BYTE_FLAG(sol->TimeFlag, 0);
    sol->TimeValid          = GET_BYTE_FLAG(sol->TimeFlag, 1);
    sol->TimeFullyResolved  = GET_BYTE_FLAG(sol->TimeFlag, 2);

    carrSoln = GET_BYTE_FLAG(sol->RtkType, 7) * 2 + GET_BYTE_FLAG(sol->RtkType, 6);
    diffSoln = GET_BYTE_FLAG(sol->RtkType, 1); 

    sol->CntNavPvt++;
    sol->seq++;
}

void UbxDecodeInit(UbxStruct *sol)
{
    sol->seq = 0;
    sol->CntNavPvt = 0;
    sol->LastCntNavPvt = 0;
}

void UbxDecodeStats(UbxStruct *sol)
{
    sol->PpsNavPvt = sol->CntNavPvt - sol->LastCntNavPvt;
    sol->LastCntNavPvt = sol->CntNavPvt;
}