/*****************************************************************
*                                                                *
*                  NAV Technology Co., Ltd.                      *
*                      Copyright 2005                            *
*                                                                *
*      Copying is prohibited except by written permission.       *
*                                                                *
******************************************************************/
/*Header file*/

#include "nvdef.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "gnss.h"
#include "eph.h"
#include "nfunc.h"
#include "mfunc.h"

#if(PC_SIM==1)
    #include <string.h>
#else
    #include "uart.h"
#endif

#define NOV_DEF
    #include "nov.h"
#undef NOV_DEF

//Declare the States
static READ_STATE  GNState[2] = {STARTUP, STARTUP};

/**********************************************************************
**
** Function: Data Command for request the Ephermeris
**
**
** Global Input: 
**                CH_GPS1_UART
**
** Global Output: 
**                None
**            
***********************************************************************/
void rqtEphem
    (
    int COMPort
    )
{
    unsigned char buf[] = {"log rawephemb onchanged\r\n  "}; 

#if(PC_SIM==0)
    charSend(COMPort, buf, 25); 
#endif

}

/**********************************************************************
**
** Function: NOV OEM GPS Initialization
**
**
** Global Input: 
**                CH_GPS1_UART
**
** Global Output: 
**                None
**            
***********************************************************************/
void initNOV
    (
    int COMPort
    )
{
	unsigned char setUnlogAll[] = {"unlogall\r\n   "};       
    unsigned char setClockEnable[] = {"clockadjust enable\r\n   "};
    unsigned char rqstEph[] = {"log rawephemb onchanged\r\n   "}; 
    unsigned char rqstRaw[] = {"log rangecmpb ontime 0.20 0\r\n   "}; 
    unsigned char rqstSlv[] = {"log bestxyzb ontime 0.20 0.1\r\n   "}; 

    // Initalize the port @ 57600 baud
#if(PC_SIM==0)
    initUART(COMPort, 1); 

    charSend(COMPort, setUnlogAll, 12); 
    charSend(COMPort, setClockEnable, 22); 
    charSend(COMPort, rqstEph, 27); 
    charSend(COMPort, rqstSlv, 33); 
    charSend(COMPort, rqstRaw, 31); 
#endif
}

/**********************************************************************
**
** Function: Read NOV Raw Measurements
**
**
** Global Input: 
**                None
**
** Global Output: 
**                None
**            
***********************************************************************/
#define MAX_VALUE       (8388608.0)
static void readRawMeas
    (
    unsigned char * Buf,                // Pointer of the buffer
    GNSS_MEAS_TYPE *Gmeas,              // Pointer of Gnss measurements
    GNSS_EPHEM_TYPE *Ephem,             // Pointer of Ephemeris
    int rcv
    )
{
    unsigned char slip_cnt, chr;
    int prn, i, numsats, snr, tmp;
    double code, carrier, dopp, lambda, adr_rolls;
    float lockTime, *L1LockTime;
    double *old_carr1;

    static float L1LockT[2*MAX_SATS_NUM];
    static double oldCarr1[2*MAX_SATS_NUM];
#if(DUAL_FREQ==1)
    float  *L2LockTime;
    double  *old_carr2;
    static float L2LockT[2*MAX_SATS_NUM];
    static double  oldCarr2[2*MAX_SATS_NUM];
#endif

    //// Set index
    if(rcv==0)
        i = 0;
    else
        i = MAX_SATS_NUM;

    //// Set pointer
    L1LockTime = &L1LockT[i];
    old_carr1 = &oldCarr1[i];
#if(DUAL_FREQ==1)
    L2LockTime = &L2LockT[i];
    old_carr2 = &oldCarr2[i];
#endif

    //// Get the satellite numbers
    Gmeas->Num_Sats = 0;
    numsats = (int)(Buf[0]);

#if(PC_SIM==1)                        
    fprintf(OUTH3, "%d\t", numsats);
#endif

    for(i=0; i<numsats && i<20; i++)
    {    
        //// Measurement effective check
        chr = (Buf[5+i*24]&0x14);
        if(chr == 0x14)
        {        
            if((Buf[6+i*24]&0x60) == 0)
            {
                // Buffer PRN of SVs
                Gmeas->PRN[Gmeas->Num_Sats++] = (Buf[21+i*24] & 0x1f);                
                prn = (int)(Buf[21+i*24] & 0x1f);                
                lambda = LAMBDA1;          
            }
            else if((Buf[6+i*24]&0x60) == 0x20)
                lambda = LAMBDA2; 
            else
                continue;         
                                                                
            /// Raw code decoding           
            code =  (double)(Buf[11+i*24])/2048.0 + (double)(Buf[12+i*24])/8.0 + (double)(Buf[13+i*24])*32.0 + 
                    (double)(Buf[14+i*24])*8192.0 + (double)(Buf[15+i*24])*2097152.0;
                       
            /// Carrier measurements decoding    
            tmp = SumOr(&Buf[16+24*i], 4);        
            carrier = (double)(tmp)/256.0;    
            adr_rolls = floor((code/lambda + carrier) / MAX_VALUE + 0.5);        

            carrier -= MAX_VALUE * adr_rolls;         
            carrier *= (-lambda);
            
            /// Signal Noise Ratio        
            snr = (Buf[24+i*24]&0xe0)/32 + (Buf[25+i*24]&0x03)*8 + 20;
            
            // Lock Time decoding        
            lockTime = (float)((double)(Buf[22+i*24])/32.0 + (double)(Buf[23+i*24]) * 8.0 + (double)(Buf[24+i*24]&0x1f) * 2048.0);
    
            /// Slip count
            if(lambda == LAMBDA1)
            {
                if(lockTime>L1LockTime[prn])
                    slip_cnt = Gmeas->L1Meas[prn].Slip_Cnt;
                else
                    slip_cnt = Gmeas->L1Meas[prn].Slip_Cnt+1;
                // Save for future epoch
                L1LockTime[prn] = lockTime;

                // Apply delta range
                dopp = carrier - old_carr1[prn];
                // Save for the next epoch
                old_carr1[prn] = carrier;
                
                if(Gmeas->L1Meas[prn].Count<1.0)
                    carrier = old_carr1[prn] + floor((code-old_carr1[prn])/LAMBDA1+0.5) * LAMBDA1;
                else
                    // Final Inetgrated Carrier Measurements in Meters
                    carrier = Gmeas->L1Meas[prn].Carrier + dopp;

                // measurement validation
                rawMeasValid( prn, snr, code, carrier, Gmeas->Time, Gmeas->LastTime,
                              0.05, slip_cnt, TRUE, TRUE, &Gmeas->L1Meas[prn], Ephem );
            }
#if(DUAL_FREQ==1)
            else 
            {
                if(lockTime>L2LockTime[prn])
                    slip_cnt = Gmeas->L2Meas[prn].Slip_Cnt;
                else
                    slip_cnt = Gmeas->L2Meas[prn].Slip_Cnt+1;
                
                L2LockTime[prn] = lockTime;

                // Apply delta range
                dopp = carrier - old_carr2[prn];
                // Save for the next epoch
                old_carr2[prn] = carrier;

                if(Gmeas->L2Meas[prn].Count<1.0)
                    carrier = old_carr2[prn] + floor((code-old_carr2[prn])/LAMBDA2+0.5) * LAMBDA2;
                else
                    // Final Inetgrated Carrier Measurements in Meters
                    carrier = Gmeas->L2Meas[prn].Carrier + dopp;
                
                // measurement validation
                rawMeasValid( prn, snr, code, carrier, Gmeas->Time, Gmeas->LastTime,
                              0.05, slip_cnt, TRUE, TRUE, &Gmeas->L2Meas[prn], Ephem );
            }
#endif
        }
        else
        {
            if((Buf[6+i*24]&0x60) == 0)
            {
                // Set the invalid FLAG of Both L1
                Gmeas->L1Meas[prn].Code_Valid = FALSE;
                Gmeas->L1Meas[prn].Phase_Valid = FALSE;
            }

#if(DUAL_FREQ==1)
            // Set the invalid FLAG of Both L2
            Gmeas->L2Meas[prn].Code_Valid = FALSE;
            Gmeas->L2Meas[prn].Phase_Valid = FALSE;
#endif
        }
    }
    
    // Save the last time
    if(Gmeas->Num_Sats>0)
       Gmeas->LastTime =  Gmeas->Time;
}


/**********************************************************************
**
** Function: Read NOV GPS PVT Solution Measurement
**
**
** Global Input: 
**                None
**
** Global Output: 
**                None
**            
***********************************************************************/
void readSolution
    (
    unsigned char * Buf,                // Pointer of the buffer
    GNSS_SOLUTION_TYPE *Solv            // Pointer of Gnss Solution
    )
{
    int i, j;
    double vel_xyz[3];
    unsigned char sol_status, pos_type;        
    
    Solv->numSats = (int)(Buf[105]&0x1f);
    
    sol_status = Buf[0];
    pos_type = Buf[4];

    Solv->mode = MODE_NONE;        
    
    if(sol_status==0)
    {
        if(pos_type==16)
            Solv->mode = MODE_STANDALONE;        
        else if((pos_type>=17) && (pos_type<=20))
            Solv->mode = MODE_CD;        
        else if((pos_type>=32) && (pos_type<=34))
            Solv->mode = MODE_NF;
        else if((pos_type>=48) && (pos_type<=50))
            Solv->mode = MODE_NI;
    }
        
#if(PC_SIM==1)                        
    fprintf(OUTH3, "%d\t%d\t", Solv->numSats, Solv->mode);
#endif

    for(i=0;i<3;i++)
    {
        //Solv->xyz[i] = *((double *)(Buf+8+i*8));
        memcpy(Solv->xyz+i, Buf+8+i*8, sizeof(double));
        //Solv->vel_xyz[i] = *((double *)(Buf+52+i*8));
        memcpy(vel_xyz+i, Buf+52+i*8, sizeof(double));         
    }

    //// Transition from ECEF to LLH
    calc_xyz2LLH_Rt2e(Solv->xyz, Solv->llh, Solv->Rt2e, Solv->Sl2t);

    //// Solution on each axis
    Solv->speed = 0.0;