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

#include "nvdef.h"
#include <math.h>
    
#define EPH_DEF
    #include "eph.h"
#undef EPH_DEF

/**********************************************************************
**
** Function: Orbital Parameters calculation
**           Computes various orbital parameter values needed to compute
**           a position interpolation point for a given SV.                                  
**
**
** Global Input: 
**                agps
**
** Global Output: 
**                None
**            
***********************************************************************/
static void calcOrbParam
    (
    SUBFRAME_ARRAY_TYPE *sub2,
    SUBFRAME_ARRAY_TYPE *sub3,
    ORBIT_PARAM_TYPE *orbt_Param
    )
{
    unsigned int U_E;    
    int tmp;
    double A, Root_A;
    double Del_N, N0;
    double W;
    double E_Square;

    union 
    { 
        unsigned int U_M0;
        int S_M0;
    }M0;

    union 
    { 
        unsigned int U_W0;
        int S_W0;
    }W0;

    union 
    { 
        unsigned int U_I0;
        int S_I0;
    }I0;

    union 
    { 
        unsigned int U_W;
        int S_W;
    }W1;

    union 
    { 
        unsigned int U;
        int S;
    }I_Dot;

    union 
    { 
        unsigned int U;
        int   S;
    }Omega_Dot;
    
    
    orbt_Param->Toe = (float)(sub2->Wd3_10[7] >> 14) * TOE_DELTA;

    // MD TBD if check is needed 
    U_E = (unsigned int)(sub2->Wd3_10[4]>>6) + (unsigned int)((sub2->Wd3_10[3] & 0x3FC0)<<18);

    orbt_Param->E = (double)(U_E) * E_24_LSB_DELTA;


    orbt_Param->Crs = (double)((int)(sub2->Wd3_10[0]<<10)&(int)(0xFFFF0000)) * CRS_DELTA / ((double)(1ul<<16));

    M0.U_M0 = (unsigned int)(sub2->Wd3_10[2]>>6) + (unsigned int)((sub2->Wd3_10[1]&0x3FC0)<<18);

    orbt_Param->M0 = (double)(M0.S_M0) * M0_24_LSB_DELTA;

    orbt_Param->Cuc = (double)((int)(sub2->Wd3_10[3]<<2)&(int)(0xFFFF0000)) * CUC_DELTA / ((double)(1ul<<16));

    orbt_Param->Cus = (double)((int)(sub2->Wd3_10[5]<<2)&(int)0xFFFF0000) * CUS_DELTA / ((double)(1ul<<16));

    orbt_Param->Cic = (double)((int)(sub3->Wd3_10[0]<<2)&(int)0xFFFF0000) * CIC_DELTA / ((double)(1ul<<16));

    W0.U_W0 = ((unsigned int)(sub3->Wd3_10[1] >> 6) + ((unsigned int)((sub3->Wd3_10[0] & 0x3FC0) << 18)) );

    orbt_Param->Omega_0 = (double)(W0.S_W0) * OMEGA_0_24_LSB_DELTA;

    orbt_Param->Cis = (double)((int)(sub3->Wd3_10[2]<<2)&(int)(0xFFFF0000)) * CIS_DELTA / ((double)(1ul<<16));
        
    I0.U_I0 = (unsigned int)(sub3->Wd3_10[3]>>6) + (unsigned int)((sub3->Wd3_10[2] & 0x3FC0)<<18);

    orbt_Param->I_0 = (double)(I0.S_I0) * I0_24_LSB_DELTA;

    orbt_Param->Crc = (double)((int)(sub3->Wd3_10[4]<<2) & (int)(0xFFFF0000)) * CRC_DELTA / ((double)(1ul<<16));

    tmp = ((sub3->Wd3_10[5]>>6)&0x00FFFFFF)|((sub3->Wd3_10[4]<<18)&0xFF000000);
    orbt_Param->Omega = (double)(tmp) * W_24_LSB_DELTA; 

    Omega_Dot.U = (unsigned int)((sub3->Wd3_10[6]>>6) * SCALE_UP_OMEGA_DOT);

    orbt_Param->Omega_Dot =(double)(Omega_Dot.S) * OMEGA_DOT_DELTA;

    I_Dot.U = (unsigned int)((sub3->Wd3_10[7]<<10) & 0xFFFC0000);

    orbt_Param->I_Dot = (double)(I_Dot.S) *(I_DOT_DELTA/(double)(1ul<<18));
    Root_A = (double)((unsigned int)(sub2->Wd3_10[6] >> 6) + (unsigned int)((sub2->Wd3_10[5]&0x3FC0)<<18))
            * ROOT_A_24_LSB_DELTA;

    orbt_Param->Root_A = Root_A;
    Del_N = (double)((int)(sub2->Wd3_10[1]<<2)&(int)(0xFFFF0000)) * DEL_N_DELTA / ((double)(1ul<<16));
    W1.U_W = (unsigned int)(sub3->Wd3_10[5]>>6) + (unsigned int)((sub3->Wd3_10[4]&0x3FC0)<<18);
    W = (double)(W1.S_W) * W_24_LSB_DELTA;

    // Compute derived data 
    A = Root_A * Root_A;

    // Find the orbit's semi-major axis
    N0 = SQRT_EGP / (Root_A * A);

    // Find the mean motion 
    orbt_Param->N = N0 + Del_N;

    // Compute corrected mean motion
    orbt_Param->Sin_W = sin(W);
    orbt_Param->Cos_W = cos(W);

    // Compute the factors of the true anomaly series expansion
    E_Square = orbt_Param->E * orbt_Param->E;
    orbt_Param->C0 = A * (1.0 - E_Square);

    // Compute the C0 term
    orbt_Param->Curve_Interval = (char)(sub2->Wd3_10[7] >> 13) & 0x1;
    orbt_Param->Valid_Ephem_Decoded = TRUE;
    orbt_Param->SV_Data_Source = EPHEMERID;
    orbt_Param->IODE = sub3->Wd3_10[7] >> 22;
}

/**********************************************************************
**
** Function: SVs Clock Parameters calculation
**           Computes various clock parameter values needed to compute
**           a position interpolation point for a given SV.                                  
**                                                                      
**
** Global Input: 
**                agps
**
** Global Output: 
**                None
**            
***********************************************************************/
static void calcClkParam
    (
    SUBFRAME_ARRAY_TYPE  *sub1,
    CLOCK_PARAM_TYPE  *clk_Param
    )
{
    union 
    {
        unsigned int U;
        int   S;
    }AF0;

    AF0.U = (unsigned int)((sub1->Wd3_10[7]>>8) * SCALE_UP_AF0);

    clk_Param->Af0 = (double)(AF0.S * AF0_DELTA);

    clk_Param->Af1 = (double)((int)(sub1->Wd3_10[6]<<10)&(int)(0xFFFF0000)) * AF1_DELTA / ((double)(1ul<<16));

    clk_Param->Af2 = (double)((char)(sub1->Wd3_10[6]>>22) * AF2_DELTA);

    clk_Param->Toc = (double)((unsigned int)(sub1->Wd3_10[5]<<10)&(unsigned int)(0xFFFF0000)) * TOC_DELTA / ((double)(1ul<<16));

    clk_Param->Tgd = (double)((char)(sub1->Wd3_10[4]>>6)&(char)(0xFF)) * TGD_DELTA;
}

/**********************************************************************
**
** Function: Read Raw ephemeris measurements
**           Computes various clock parameter values needed to compute
**           a position interpolation point for a given SV.                                  
**                                                                      
**
** Global Input: 
**                agps
**
** Global Output: 
**                None
**            
***********************************************************************/
void readRawEphemMeas
    (
    RAW_EPHEM_TYPE blks,        // Raw Ephemeris blocks
    GNSS_EPHEM_TYPE *ephem      // Pointer of Ephemeris
    )
{
    SUBFRAME_ARRAY_TYPE sub123[3];

    int i, j, prn;

    for(i=0; i<3; i++)
    {
        for(j=0; j<8; j++)
        {   // Pack 3 bytes to "bit6 ~ bit30"
            sub123[i].Wd3_10[j] = ((unsigned int)blks.Bytes[24*i + 3*j] <<22) +
                                  ((unsigned int)blks.Bytes[24*i + 3*j + 1] <<14) +
                                  ((unsigned int)blks.Bytes[24*i+ 3*j + 2]<<6);
        }
    }

    // Decode PRN number 
    prn = blks.Svs + 1;

    // Calculate Orbital Paramters
    calcOrbParam(&sub123[1], &sub123[2], &ephem->Orbt_Param[prn]);

    // Calculate Clock Paramters
    calcClkParam(&sub123[0], &ephem->Clk_Param[prn]);

    // Set valid FLAG
    ephem->Ephem_Valid[prn] = TRUE;
}