/*
 *  xm_gps.c
 *  By SQ2IPS
 */

#include "xm_gps.h"
#include "main.h"
#include "config.h"

#include <string.h>

const uint8_t syncChain[] = {0xAA, 0xAA, 0xAA, 0x03};

uint32_t oldTime = 0;
float oldAlt = 0;
float oldLat = 0;
float oldLon = 0;

void ParseXM(XMDATA *GpsData, uint8_t *buffer, uint8_t position){
    GpsData->Fix = *(uint8_t *)(buffer+4+position);

    int32_t Lat;
    memcpy(&Lat, buffer + 5 + position, sizeof(int32_t));
    Lat = ((Lat & 0xFF000000) >> 24) | 
                ((Lat & 0x00FF0000) >> 8)  | 
                ((Lat & 0x0000FF00) << 8)  | 
                ((Lat & 0x000000FF) << 24);
    int32_t Lon;
    memcpy(&Lon, buffer + 9 + position, sizeof(int32_t));
    Lon = ((Lon & 0xFF000000) >> 24) | 
                ((Lon & 0x00FF0000) >> 8)  | 
                ((Lon & 0x0000FF00) << 8)  | 
                ((Lon & 0x000000FF) << 24);

    uint32_t Alt;
    memcpy(&Alt, buffer + 12 + position, sizeof(uint32_t)); // from uint24_t
    Alt = ((Alt & 0xFF000000) >> 24) | 
                ((Alt & 0x00FF0000) >> 8)  | 
                ((Alt & 0x0000FF00) << 8);
    
    GpsData->Lat = Lat/1e6;
    GpsData->Lon = Lon/1e6;
    GpsData->Alt = Alt/1e2;

    memcpy(&GpsData->Time, buffer + 21 + position, sizeof(uint32_t)); // from uint24_t
    GpsData->Time = ((GpsData->Time & 0xFF000000) >> 24) |
                ((GpsData->Time & 0x00FF0000) >> 8) |
                ((GpsData->Time & 0x0000FF00) << 8);

    if((GpsData->Time - oldTime) >= AscentRateTime){
        if(oldTime!=0){
            GpsData->AscentRate = (GpsData->Alt-oldAlt)/(GpsData->Time-oldTime);
            
            //float a = pow(sin((fabs(GpsData->Lat - oldLat)*90)/M_PI), 2) + cos(((GpsData->Lat)*180)/M_PI) * cos((oldLat*180)/M_PI) * pow(sin((fabs(GpsData->Lon - oldLon)*90)/M_PI),2);
            //float d = 2*6371*atan2(sqrt(a), sqrt(1-a));
            //GpsData->GroundSpeed = d;
            //GpsData->GroundSpeed = sqrt(pow(d, 2) + pow(GpsData->Alt-oldAlt, 2))/(GpsData->Time-oldTime);
        }else{
            GpsData->AscentRate = 0;
            GpsData->GroundSpeed = 0;
        }
        oldTime = GpsData->Time;
        oldAlt = GpsData->Alt;
        oldLat = GpsData->Lat;
        oldLon = GpsData->Lon;
    }

    GpsData->Hours = (GpsData->Time/3600)%24;
    GpsData->Minutes = (GpsData->Time/60)%60;
    GpsData->Seconds = GpsData->Time%60;

    //memcpy(&GpsData->Sats, buffer + 27 + position, sizeof(uint8_t)); // not sure of this byte meaning (always 12)
    for(GpsData->Sats = 0; GpsData->Sats<16; GpsData->Sats++){
        if(buffer[position+28+GpsData->Sats] == 0){
            break;
        }
    }
}

int8_t getPosition(uint8_t *buffer){
    for(uint8_t pos = 0; pos<=FrameLen+1; pos++){
        bool sync = true;
        for(uint8_t syncCount = 0; syncCount<sizeof(syncChain)/sizeof(syncChain[0]); syncCount++){
            if(buffer[pos+syncCount] != syncChain[syncCount]){
                sync = false;
            }
        }
        if(sync){
            return pos;
        }
    }
    return -1;
}

void parseXMframe(XMDATA *GpsData, uint8_t *buffer){
    int8_t pos = getPosition(buffer);
    if(pos != -1){
        ParseXM(GpsData, buffer, pos);
    }else{
        GpsData->Fix = 0;
    }
}