/** Author: Tomas Vankat */
#include <stdio.h>
#include <stdlib.h>
#include <stddef.h>
#include <inttypes.h>
#include <math.h>
#include "process_GALILEO_data.h"
//vyber frekvence, kvuli rozdilu vypoctu BGD
#define E1
//#define E5b

int process_GALILEO_data(uint64_t * buffer, uint8_t SV_number){
    uint8_t Stranka[240]; //pole pro celou dekodovanou stranku
    int i,typeword,k;
    uint64_t nav_msg[16]; //pro kopii bufferu

    if(initialization==0){ //pocatecni vynulovani promennych
        GST[SV_number-1].time=0;
        GST[SV_number-1].precise_time=0;
        GST[SV_number-1].week=0;
        for( i=0 ; i<36 ; i++){
            WordType1[i].IODnav=0;
            WordType1[i].toe=0;
            WordType1[i].Mo=0;
            WordType1[i].e=0;
            WordType1[i].sqrtA=0;
            WordType2[i].IODnav=0;
            WordType2[i].OMEGA_0=0;
            WordType2[i].io=0;
            WordType2[i].omega=0;
            WordType2[i].ii=0;
            WordType3[i].IODnav=0;
            WordType3[i].omega_dot=0;
            WordType3[i].delta_n=0;
            WordType3[i].C_uc=0;
            WordType3[i].C_us=0;
            WordType3[i].C_rc=0;
            WordType3[i].C_rs=0;
            WordType3[i].SISA=0;
            WordType4[i].IODnav=0;
            WordType4[i].SVID=0;
            WordType4[i].C_ic=0;
            WordType4[i].C_is=0;
            WordType4[i].t_0c=0;
            WordType4[i].a_f0=0;
            WordType4[i].a_f1=0;
            WordType4[i].a_f2=0;
            WordType5[i].a_i0=0;
            WordType5[i].a_i1=0;
            WordType5[i].a_i2=0;
            WordType5[i].Region1=0;
            WordType5[i].Region2=0;
            WordType5[i].Region3=0;
            WordType5[i].Region4=0;
            WordType5[i].Region5=0;
            WordType5[i].BGDa=0;
            WordType5[i].BGDb=0;
            WordType5[i].E5b_Hs=0;
            WordType5[i].E1B_HS=0;
            WordType5[i].E5b_DVs=0;
            WordType5[i].E1B_DVs=0;
            WordType5[i].WN=0;
            WordType5[i].TOW=0;
            WordType6[i].A0=0;
            WordType6[i].A1=0;
            WordType6[i].deltat_LS=0;
            WordType6[i].t_ot=0;
            WordType6[i].WN_0t=0;
            WordType6[i].WN_LSF=0;
            WordType6[i].DN=0;
            WordType6[i].deltat_LSF=0;
            WordType6[i].TOW=0;
            WordType7[i].IODa=0;
            WordType7[i].WNa=0;
            WordType7[i].t_0a=0;
            WordType7[i].SVID1=0;
            WordType7[i].delta_sqrtA=0;
            WordType7[i].e=0;
            WordType7[i].omega=0;
            WordType7[i].d_i=0;
            WordType7[i].omega_0=0;
            WordType7[i].omega_dot=0;
            WordType7[i].M_0=0;
            WordType8[i].IODa=0;
            WordType8[i].a_f0=0;
            WordType8[i].a_f1=0;
            WordType8[i].E5b_HS=0;
            WordType8[i].E1B_HS=0;
            WordType8[i].SVID2=0;
            WordType8[i].delta_sqrtA=0;
            WordType8[i].e=0;
            WordType8[i].omega=0;
            WordType8[i].d_i=0;
            WordType8[i].omega_0=0;
            WordType8[i].omega_dot=0;
            WordType9[i].IODa=0;
            WordType9[i].WNa=0;
            WordType9[i].t_0a=0;
            WordType9[i].M_0=0;
            WordType9[i].a_f0=0;
            WordType9[i].a_f1=0;
            WordType9[i].E5b_HS=0;
            WordType9[i].E1B_HS=0;
            WordType9[i].SVID3=0;
            WordType9[i].delta_sqrtA=0;
            WordType9[i].e=0;
            WordType9[i].omega=0;
            WordType9[i].d_i=0;
            WordType10[i].IODa=0;
            WordType10[i].omega_0=0;
            WordType10[i].omega_dot=0;
            WordType10[i].M_0=0;
            WordType10[i].a_f0=0;
            WordType10[i].a_f1=0;
            WordType10[i].E5b_HS=0;
            WordType10[i].E1B_HS=0;
            WordType10[i].A_0G=0;
            WordType10[i].A_1G=0;
            WordType10[i].t_0G=0;
            WordType10[i].WN_0G=0;
            WordType0[i].Time=0;
            WordType0[i].WN=0;
            WordType0[i].TOW=0;
        }
    initialization=1;
    }
    GST[SV_number-1].time=GST[SV_number-1].time+0.004;
        //inkrementace casu druzice
    GST[SV_number-1].precise_time=GST[SV_number-1].precise_time+4000000000; //inkrementace casu druzice
    for( i=0 ; i<240 ; i++){
        Stranka[i]=2;
    }
    for(i=0;i<16;i++){
        nav_msg[i]=buffer[i]; //zkopirovani bufferu
    }
    k=Dekodovani(nav_msg,Stranka);
    if(k!=1){
        return -6; //nenalezen zacatek stranky
    }
    typeword=0;//prepsani Typeword z pole do samostatne promenne
    for( i=2 ; i<8 ; i++ ){
        if(Stranka[i]>1){
            return -5;//nebyl prirazen nit
        }
        typeword=typeword<<1;
        if(Stranka[i]==1){
            typeword++;
        }
    }
    if((typeword<0)||(typeword>10)){
        return -4;//chyba nebo rezervovane dat. slovo
    }
    if(CRC(Stranka)!=0){
        return -3;//CRC se nevyvulovalo - v datech je chyba
    }
    switch (typeword){//zpracovani podle typeword
    case 1:
        sortWT1(Stranka,SV_number);
        break;
    case 2:
        sortWT2(Stranka,SV_number);
        break;
    case 3:
        sortWT3(Stranka,SV_number);
        break;
    case 4:
        sortWT4(Stranka,SV_number);
        break;
    case 5: //obsahuje GST
        sortWT5(Stranka,SV_number);
        GST[SV_number-1].time=WordType5[SV_number-1].TOW + (260*0.004);
        GST[SV_number-1].precise_time=WordType5[SV_number-1].TOW*1000000000000 + 1040000000000;//(260*4000000000);
        GST[SV_number-1].week=WordType5[SV_number-1].WN;
        break;
    case 6: //obsahuje TOW
        sortWT6(Stranka,SV_number);
        GST[SV_number-1].time=WordType6[SV_number-1].TOW + (260*0.004);
        GST[SV_number-1].precise_time=WordType6[SV_number-1].TOW*1000000000000 + 1040000000000;//(260*4000000000);
        break;
    case 7:
        sortWT7(Stranka,SV_number);
        break;
    case 8:
        sortWT8(Stranka,SV_number);
        break;
    case 9:
        sortWT9(Stranka,SV_number);
        break;
    case 10:
        sortWT10(Stranka,SV_number);
        break;
    case 0: //obsahuje GST
        sortWT0(Stranka,SV_number);
        GST[SV_number-1].time=WordType0[SV_number-1].TOW + (260*0.004);
        GST[SV_number-1].precise_time=WordType0[SV_number-1].TOW*1000000000000 + 1040000000000;//(260*4000000000);
        GST[SV_number-1].week=WordType0[SV_number-1].WN;
        break;
    default:
        break;
    }
    store_almanac_data(SV_number,typeword);//ulozeni dat almanachu
    return typeword;
}

double User_pos(SVID){//nastin funkce vypoctu polohy
    SV_coordinates(SVID);
    system_rotation(SVID, 0.085); //1 odhad 85ms
    /*prange_calc();
    while((fabs(prev_prange-prange) > tolerance)){
        receiver_pos();
        system_rotation(SVID, time0[SVID-1]);
        Iono_corr();
        Tropo_corr();
        prange_corr();
    }
    receiver_pos();*/
    return 0;
}

void system_rotation(uint8_t SVID, double spr_time){ //spr_time .. doba sireni signalu mezi vyslanim a prijmem
    double theta = 0;
    double X,Y,Z;
    theta = omega_e * (((double)GST[SVID-1].precise_time/1000000000000+spr_time) - ((double)GST[SVID-1].precise_time/1000000000000));
    X = (cos(theta)*SV_coor[SVID-1].X) + (sin(theta)*SV_coor[SVID-1].Y); // + 0*SVcoordin[SVID-1].Z;
    Y = (-sin(theta)*SV_coor[SVID-1].X) + (cos(theta)*SV_coor[SVID-1].Y); // + 0*SVcoordin[SVID-1].Z;
    Z = 1*SV_coor[SVID-1].Z; // + 0*SVcoordin[SVID-1].X + 0*SVcoordin[SVID-1].Y
    SV_coor[SVID-1].X=X;
    SV_coor[SVID-1].Y=Y;
    SV_coor[SVID-1].Z=Z;
}

double GST_UTC(uint8_t SVID){
    double t_E=GST[SVID-1].time;
    double delta_t_utc,W;
    //case a - serizovany cas neni v minulosti a nejsme v rozmezi +-6hodin od serizeni
    if ( ((GST[SVID-1].week%256) < WordType6[SVID-1].WN_LSF) || /*bud je aktualni tyden mensi*/
        ( ((GST[SVID-1].week%256) == WordType6[SVID-1].WN_LSF) && ((((int)GST[SVID-1].time/86400)+1) < WordType6[SVID-1].DN)) || /*nebo je tyden stejny a den mensi*/
        ( ((GST[SVID-1].week%256) == WordType6[SVID-1].WN_LSF) && ((((int)GST[SVID-1].time/86400)+1) == WordType6[SVID-1].DN) && (((fmod(GST[SVID-1].time,86400))/3600) < 18 )) ) /*nebo je tyden i den stejny a jsme vic jak 6 hodin pred*/
    {
        delta_t_utc = WordType6[SVID-1].deltat_LS + WordType6[SVID-1].A0*pow(2,-30) + WordType6[SVID-1].A1*pow(2,-50)*(t_E - WordType6[SVID-1].t_ot*3600 + 604800*(WordType5[SVID-1].WN - WordType6[SVID-1].WN_0t));
        UTC.daytime=fmod((t_E - delta_t_utc),86400);
        return UTC.daytime;
    }
    //case b - jsme v rozmezi +-6 hodin od serizeni
    if ( (((GST[SVID-1].week%256) == WordType6[SVID-1].WN_LSF) && ((((int)GST[SVID-1].time/86400)+1) == WordType6[SVID-1].DN) && (((fmod(GST[SVID-1].time,86400))/3600) >= 18)) ||  /*bud je tyden i den stejny a cas min nez 6 hodin ke konci dne*/
        (((GST[SVID-1].week%256) == WordType6[SVID-1].WN_LSF) && ((((int)GST[SVID-1].time/86400)+1) == (WordType6[SVID-1].DN+1)) && (((fmod(GST[SVID-1].time,86400))/3600) <= 6))  || /*nebo tyden stejny a den prave vetsi o 1 a cas mensi nez 6. hodina*/
        (((GST[SVID-1].week%256) == (WordType6[SVID-1].WN_LSF+1)) && ((((int)GST[SVID-1].time/86400)+1) == 1) && (((fmod(GST[SVID-1].time,86400))/3600) <= 6)) ) /*nebo je tyden o 1 vetsi a den je prvni v tydnu a cas mensi nez 6 hodin*/
    {
        delta_t_utc = WordType6[SVID-1].deltat_LS + WordType6[SVID-1].A0*pow(2,-30) + WordType6[SVID-1].A1*pow(2,-50)*(t_E - WordType6[SVID-1].t_ot*3600 + 604800*(WordType5[SVID-1].WN - WordType6[SVID-1].WN_0t));
        W =(fmod( ( t_E - delta_t_utc - 43200) , 86400) + 43200 );
        UTC.daytime =fmod(W,( 86400 + WordType6[SVID-1].deltat_LSF - WordType6[SVID-1].deltat_LS ));
        return UTC.daytime;
    }
    //case c - serizovany cas je v minulosti (uz probehl) a nejsme v rozmezi 6hodin od serizeni
    if ( ((GST[SVID-1].week%256) > (WordType6[SVID-1].WN_LSF+1)) || /*bud je aktualni tyden vetsi o vic nez 1 nez tyden kdy se aplikuje leap second*/
        ( ((GST[SVID-1].week%256) == (WordType6[SVID-1].WN_LSF+1)) && ((((int)GST[SVID-1].time/86400)+1) > 1)) || /*nebo je tyden o 1 vetsi a den neni prvni v tydnu*/
        ( ((GST[SVID-1].week%256) == (WordType6[SVID-1].WN_LSF+1)) && ((((int)GST[SVID-1].time/86400)+1) == 1) && (((fmod(GST[SVID-1].time,86400))/3600) > 6 )) || /*nebo je tyden vetsi o 1 a den 1. v tydnu a cas vetsi nez 6 rano*/
        ( ((GST[SVID-1].week%256) == (WordType6[SVID-1].WN_LSF)) && ((((int)GST[SVID-1].time/86400)+1) > (WordType6[SVID-1].DN+1))) || /*nebo je tyden stejny den vetsi o vice nez 1*/
        ( ((GST[SVID-1].week%256) == (WordType6[SVID-1].WN_LSF)) && ((((int)GST[SVID-1].time/86400)+1) == (WordType6[SVID-1].DN+1)) && (((fmod(GST[SVID-1].time,86400))/3600) > 6) ) ) /*nebo je tyden stejny den vetsi o 1 a jsem vic jak 6 hodin po*/
   {
        delta_t_utc = WordType6[SVID-1].deltat_LSF + WordType6[SVID-1].A0*pow(2,-30) + WordType6[SVID-1].A1*pow(2,-50)*( t_E - WordType6[SVID-1].t_ot*3600 + 604800*(WordType5[SVID-1].WN - WordType6[SVID-1].WN_0t));
        UTC.daytime =fmod(( t_E - delta_t_utc ), 86400);
        return UTC.daytime;
    }
    return -1;
}

double SV_coordinates(uint8_t SVID){
    double A,n0,tk,n,M,v,delta_u,delta_r,delta_i,u,r,i,x_,y_,Omega,Phi,E,prevE,prevtk;
    int kep_iter=0,tk_iter=0;
    if((WordType1[SVID-1].IODnav==WordType2[SVID-1].IODnav) && (WordType2[SVID-1].IODnav==WordType3[SVID-1].IODnav) && (WordType3[SVID-1].IODnav==WordType4[SVID-1].IODnav)){
        //IOD vsech efemerid se rovnaji
        if(WordType1[SVID-1].sqrtA==0){
            //nejsou data
            SV_coor[SVID-1].X=0;
            SV_coor[SVID-1].Y=0;
            SV_coor[SVID-1].Z=0;
            return -2;
        }
    }else{ //IOD efemerid se nerovnaji
        SV_coor[SVID-1].X=0;
        SV_coor[SVID-1].Y=0;
        SV_coor[SVID-1].Z=0;
        return -1;
    }
    A=pow((WordType1[SVID-1].sqrtA*pow(2,-19)),2);
    n0=sqrt((my/(pow(A,3))));
    n=n0+(PI*WordType3[SVID-1].delta_n*pow(2,-43));
    tk=(double)GST[SVID-1].precise_time/1000000000000-60*WordType1[SVID-1].toe;
    while((fabs(prevtk-tk)) > 0.000000001){
        M=PI*WordType1[SVID-1].Mo*pow(2,-31)+n*tk;
        E=M;
        kep_iter=0;
        while((fabs((prevE-E)) > 0.0000000001)){
            prevE=E;
            E=M+WordType1[SVID-1].e*pow(2,-33)*sin(prevE);
            kep_iter++;
            if(kep_iter>4){ //max 5 iteraci
                break;
            }
        }
        prevtk=tk;
        tk=(double)GST[SVID-1].precise_time/1000000000000-Time_Correction(SVID,E)-60*WordType1[SVID-1].toe;
        tk_iter++;
        if(tk_iter>4){ //max 5 iteraci
                break;
            }
        }
    v=atan2(((sqrt(1-pow(WordType1[SVID-1].e*pow(2,-33),2))*sin(E))/(1-WordType1[SVID-1].e*pow(2,-33)*cos(E))) , ((cos(E)-WordType1[SVID-1].e*pow(2,-33))/(1-WordType1[SVID-1].e*pow(2,-33)*cos(E))));
    Phi= v + PI*WordType2[SVID-1].omega*pow(2,-31);
    delta_u=WordType3[SVID-1].C_us*pow(2,-29)*sin(2*Phi) + WordType3[SVID-1].C_uc*pow(2,-29)*cos(2*Phi);
    delta_r=WordType3[SVID-1].C_rs*pow(2,-5)*sin(2*Phi) + WordType3[SVID-1].C_rc*pow(2,-5)*cos(2*Phi);
    delta_i=WordType4[SVID-1].C_is*pow(2,-29)*sin(2*Phi) + WordType4[SVID-1].C_ic*pow(2,-29)*cos(2*Phi);
    u=Phi+delta_u;
    r=A*(1-WordType1[SVID-1].e*pow(2,-33)*cos(E))+delta_r;
    i=PI*WordType2[SVID-1].io*pow(2,-31) + delta_i + PI*WordType2[SVID-1].ii*pow(2,-43)*tk;
    x_=r*cos(u);
    y_=r*sin(u);
    Omega=PI*WordType2[SVID-1].OMEGA_0*pow(2,-31) + (PI*WordType3[SVID-1].omega_dot*pow(2,-43)-omega_e)*tk - omega_e*WordType1[SVID-1].toe*60;
    SV_coor[SVID-1].X=x_*cos(Omega) - y_*cos(i)*sin(Omega);
    SV_coor[SVID-1].Y=x_*sin(Omega) + y_*cos(i)*cos(Omega);
    SV_coor[SVID-1].Z=y_*sin(i);
    return E;
}

void store_almanac_data(uint8_t SVID, uint8_t typeword){
//ulozeni dat z almanachu do struktury dane druzice
if(typeword==7 || typeword==8){
    if( WordType7[SVID-1].SVID1>0 && WordType7[SVID-1].SVID1<37){
        if(WordType7[SVID-1].IODa == WordType8[SVID-1].IODa){
            almanac[WordType7[SVID-1].SVID1-1].IODa=WordType7[SVID-1].IODa;
            almanac[WordType7[SVID-1].SVID1-1].WNa=WordType7[SVID-1].WNa;
            almanac[WordType7[SVID-1].SVID1-1].t_0a=WordType7[SVID-1].t_0a;
            almanac[WordType7[SVID-1].SVID1-1].SVID=WordType7[SVID-1].SVID1;
            almanac[WordType7[SVID-1].SVID1-1].delta_sqrtA=WordType7[SVID-1].delta_sqrtA;
            almanac[WordType7[SVID-1].SVID1-1].e=WordType7[SVID-1].e;
            almanac[WordType7[SVID-1].SVID1-1].omega=WordType7[SVID-1].omega;
            almanac[WordType7[SVID-1].SVID1-1].d_i=WordType7[SVID-1].d_i;
            almanac[WordType7[SVID-1].SVID1-1].omega_0=WordType7[SVID-1].omega_0;
            almanac[WordType7[SVID-1].SVID1-1].omega_dot=WordType7[SVID-1].omega_dot;
            almanac[WordType7[SVID-1].SVID1-1].M_0=WordType7[SVID-1].M_0;
            almanac[WordType7[SVID-1].SVID1-1].a_f0=WordType8[SVID-1].a_f0;
            almanac[WordType7[SVID-1].SVID1-1].a_f1=WordType8[SVID-1].a_f1;
            almanac[WordType7[SVID-1].SVID1-1].E5b_HS=WordType8[SVID-1].E5b_HS;
            almanac[WordType7[SVID-1].SVID1-1].E1B_HS=WordType8[SVID-1].E1B_HS;
        }
    }
}
if(typeword==8 || typeword==9){
    if(WordType8[SVID-1].SVID2>0 && WordType8[SVID-1].SVID2<37){
        if(WordType8[SVID-1].IODa == WordType9[SVID-1].IODa){
            almanac[WordType8[SVID-1].SVID2-1].IODa=WordType8[SVID-1].IODa;
            almanac[WordType8[SVID-1].SVID2-1].WNa=WordType9[SVID-1].WNa;
            almanac[WordType8[SVID-1].SVID2-1].t_0a=WordType9[SVID-1].t_0a;
            almanac[WordType8[SVID-1].SVID2-1].SVID=WordType8[SVID-1].SVID2;
            almanac[WordType8[SVID-1].SVID2-1].delta_sqrtA=WordType8[SVID-1].delta_sqrtA;
            almanac[WordType8[SVID-1].SVID2-1].e=WordType8[SVID-1].e;
            almanac[WordType8[SVID-1].SVID2-1].omega=WordType8[SVID-1].omega;
            almanac[WordType8[SVID-1].SVID2-1].d_i=WordType8[SVID-1].d_i;
            almanac[WordType8[SVID-1].SVID2-1].omega_0=WordType8[SVID-1].omega_0;
            almanac[WordType8[SVID-1].SVID2-1].omega_dot=WordType8[SVID-1].omega_dot;
            almanac[WordType8[SVID-1].SVID2-1].M_0=WordType9[SVID-1].M_0;
            almanac[WordType8[SVID-1].SVID2-1].a_f0=WordType9[SVID-1].a_f0;
            almanac[WordType8[SVID-1].SVID2-1].a_f1=WordType9[SVID-1].a_f1;
            almanac[WordType8[SVID-1].SVID2-1].E5b_HS=WordType9[SVID-1].E5b_HS;
            almanac[WordType8[SVID-1].SVID2-1].E1B_HS=WordType9[SVID-1].E1B_HS;
        }
    }
}
if(typeword==9 || typeword==10){
    if(WordType9[SVID-1].SVID3>0 && WordType9[SVID-1].SVID3<37){
        if(WordType9[SVID-1].IODa == WordType10[SVID-1].IODa){
            almanac[WordType9[SVID-1].SVID3-1].IODa=WordType9[SVID-1].IODa;
            almanac[WordType9[SVID-1].SVID3-1].WNa=WordType9[SVID-1].WNa;
            almanac[WordType9[SVID-1].SVID3-1].t_0a=WordType9[SVID-1].t_0a;
            almanac[WordType9[SVID-1].SVID3-1].SVID=WordType9[SVID-1].SVID3;
            almanac[WordType9[SVID-1].SVID3-1].delta_sqrtA=WordType9[SVID-1].delta_sqrtA;
            almanac[WordType9[SVID-1].SVID3-1].e=WordType9[SVID-1].e;
            almanac[WordType9[SVID-1].SVID3-1].omega=WordType9[SVID-1].omega;
            almanac[WordType9[SVID-1].SVID3-1].d_i=WordType9[SVID-1].d_i;
            almanac[WordType9[SVID-1].SVID3-1].omega_0=WordType10[SVID-1].omega_0;
            almanac[WordType9[SVID-1].SVID3-1].omega_dot=WordType10[SVID-1].omega_dot;
            almanac[WordType9[SVID-1].SVID3-1].M_0=WordType10[SVID-1].M_0;
            almanac[WordType9[SVID-1].SVID3-1].a_f0=WordType10[SVID-1].a_f0;
            almanac[WordType9[SVID-1].SVID3-1].a_f1=WordType10[SVID-1].a_f1;
            almanac[WordType9[SVID-1].SVID3-1].E5b_HS=WordType10[SVID-1].E5b_HS;
            almanac[WordType9[SVID-1].SVID3-1].E1B_HS=WordType10[SVID-1].E1B_HS;
        }
    }
}
}

void almanac_SV_coordinates(uint8_t SVID,uint8_t SVIDa,int16_t delta_sqrtA,uint16_t e,int16_t delta_i,int16_t omega_0,int16_t omega_dot,int16_t omega,int16_t M_0,uint16_t toa,uint8_t WNa){
    //vypocet polohy druzice z almanachu
    double A,n0,tk,n,M,v,u,r,i,x_,y_,Omega,Phi,E,prevE;
    int kep_iter=0;
    A=pow( ((delta_sqrtA*pow(2,-9))+sqrt(29600000)) , 2 );
    n0=sqrt((my/(pow(A,3))));
    tk=GST[SVID-1].time-600*toa;
    n=n0;
    M=PI*M_0*pow(2,-15)+n*tk;
    E=M;
    while((fabs((prevE-E)) > 0.000000000001)){
        prevE=E;
        E=M+e*pow(2,-16)*sin(prevE);
        kep_iter++;
        if(kep_iter>4){ //max 5 iteraci
                break;
            }
    }
    v=atan2(((sqrt(1-pow(e*pow(2,-16),2))*sin(E))/(1-e*pow(2,-16)*cos(E))) , ((cos(E)-e*pow(2,-16))/(1-e*pow(2,-16)*cos(E))));
    Phi= v + PI*omega*pow(2,-15);
    u=Phi;
    r=A*(1-e*pow(2,-16)*cos(E));
    i=(PI*delta_i*pow(2,-14)+0.9773843811); //0.977384rad = 56deg
    x_=r*cos(u);
    y_=r*sin(u);
    Omega=PI*omega_0*pow(2,-15) + (PI*omega_dot*pow(2,-33)-omega_e)*tk - omega_e*toa*600;
    SV_coor[SVIDa-1].X=x_*cos(Omega) - y_*cos(i)*sin(Omega);
    SV_coor[SVIDa-1].Y=x_*sin(Omega) + y_*cos(i)*cos(Omega);
    SV_coor[SVIDa-1].Z=y_*sin(i);
}

double Time_Correction(uint8_t SVID, double E){
    double delta_tr=0, delta_tsv=0;
    delta_tr=(-4.442807309*pow(10,-10))*WordType1[SVID-1].e*pow(2,-33)*WordType1[SVID-1].sqrtA*pow(2,-19)*sin(E);
    delta_tsv=WordType4[SVID-1].a_f0*pow(2,-34) + WordType4[SVID-1].a_f1*pow(2,-46)*(GST[SVID-1].time-WordType4[SVID-1].t_0c*60) + WordType4[SVID-1].a_f2*pow(2,-59)*(pow((GST[SVID-1].time-WordType4[SVID-1].t_0c*60),2))+delta_tr;
    #ifdef E1
    delta_tsv=delta_tsv-WordType5[SVID-1].BGDb*pow(2,-32);
    #endif // E1
    #ifdef E5b
    delta_tsv=delta_tsv-1.70324619362252223*WordType5[SVID-1].BGDb*pow(2,-32);
    #endif // E5b
    return delta_tsv;
}

void Deinterleaving(uint8_t Prijato[]){
uint8_t i,j,k;
uint8_t B[8][30];
k=0;
for( i=0 ; i<8 ; i++){
    for( j=0; j<30 ; j++ , k++){
        B[i][j]=Prijato[k];
    }
}
k=0;
for( j=0 ; j<30 ; j++){
    for( i=0; i<8 ; i++ , k++ ){
        Prijato[k]=B[i][j];
    }
}
}

void ViterbiDecoding(uint8_t prijato[],uint8_t nejpravdepodobnejsi[]){
struct {
    uint64_t Posloupnost[2];
    uint8_t HW;
} cesta[64],pomoc[64];
uint64_t nasobek;
uint8_t stav,i,HW1,HW2,previous1,previous2,bit;
bit=0;
cesta[0].HW=0;
cesta[0].Posloupnost[0]=0;
cesta[0].Posloupnost[1]=0;
for( i=1 ; i<64 ; i++){
    cesta[i].HW=255;
    cesta[i].Posloupnost[0]=0;
    cesta[i].Posloupnost[1]=0;
}
for(i=0; i<240 ; i=i+2){ //vnejsi cyklus pres bity
    for( stav=0 ; stav<64 ; stav++){
        pomoc[stav].HW=cesta[stav].HW;
        pomoc[stav].Posloupnost[0]=cesta[stav].Posloupnost[0];
        pomoc[stav].Posloupnost[1]=cesta[stav].Posloupnost[1];
    }
    for( stav=0 ; stav<64 ; stav++){ //vnitrni cyklus pres stavy
        if(stav>31){
            bit=1;
        }
        else{
            bit=0;
        }
        previous1=2*(stav-(32*(stav/32)));
        previous2=2*(stav-(32*(stav/32)))+1;
        HW1=0;
        HW2=0;
        //lze udelat jako (0x1111001 & (bit*64+previous1)) % 2
        //nebo misto bit*64 udelat bit<<6
        //if(prijato[i]!=((0x1111001 & (bit*64+previous1))%2)){
        //nebo:
        if(prijato[i]!=((((bit+previous1)&0x01)/0x01)^(((bit+previous1)&0x08)/0x08)^(((bit+previous1)&0x10)/0x10)^(((bit+previous1)&0x20)/0x20)^(((bit+previous1)&0x40)/0x40))){ //G1
            HW1++;
        }
        //(prijato[i]!=(!((0x1011011 & (bit*64+previous2))%2))){
        //nebo:
        if(prijato[i+1]!=(!((((bit+previous1)&0x01)/0x01)^(((bit+previous1)&0x02)/0x02)^(((bit+previous1)&0x08)/0x08)^(((bit+previous1)&0x10)/0x10)^(((bit+previous1)&0x40)/0x40)))){ //G2
            HW1++;
        }
        if(HW1==0)
            {HW2=2;}
        else if(HW1==2)
            {HW2=0;}
        else
            {HW2=1;}
        if( (pomoc[previous1].HW+HW1) >= (pomoc[previous2].HW+HW2) ){
            cesta[stav].HW=pomoc[previous2].HW+HW2;
            cesta[stav].Posloupnost[0]=pomoc[previous2].Posloupnost[0];
            cesta[stav].Posloupnost[1]=pomoc[previous2].Posloupnost[1];
        }
        else{
            cesta[stav].HW=pomoc[previous1].HW+HW1;
            cesta[stav].Posloupnost[0]=pomoc[previous1].Posloupnost[0];
            cesta[stav].Posloupnost[1]=pomoc[previous1].Posloupnost[1];
        }

        cesta[stav].Posloupnost[1]=cesta[stav].Posloupnost[1]<<1;
        if((cesta[stav].Posloupnost[0]&0x8000000000000000) > 0){
            cesta[stav].Posloupnost[1]=cesta[stav].Posloupnost[1]+1;
        }
        cesta[stav].Posloupnost[0]=cesta[stav].Posloupnost[0]<<1;
        cesta[stav].Posloupnost[0]=cesta[stav].Posloupnost[0]+(stav/32);
    }
}
//prepsani dekodovanych bitu do pole
nasobek=0x0080000000000000;
for( i=0 ; i<120 ; i++)
{
    if(nasobek==0){
        nasobek=0x8000000000000000;
    }
    if(i<56){
        if((cesta[0].Posloupnost[1]&nasobek)>0){
            nejpravdepodobnejsi[i]=1;
        }
        else{
            nejpravdepodobnejsi[i]=0;
        }
        nasobek=nasobek>>1;
    }
    else{
        if((cesta[0].Posloupnost[0]&nasobek)>0){
            nejpravdepodobnejsi[i]=1;
        }
        else{
            nejpravdepodobnejsi[i]=0;
        }
        nasobek=nasobek>>1;
    }
}
}

int Dekodovani(uint64_t nav_msg[], uint8_t Stranka[]){ //uplatnuje deinterleaving a dekodovani
    uint64_t posun;
    uint8_t Prijato[240];
    uint8_t Dekodovano[120];
    int16_t i,k;
    uint8_t inverze=0;
    uint8_t zacatek=0;
        if ((nav_msg[0]&0x00000000000003FF)==0x0000000000000160){
            //na zacatku je synch. slovo
            if ((((nav_msg[3]&0xFC00000000000000)==(0x8000000000000000))) && (((nav_msg[4])&(0x000000000000000F))==(0x0000000000000005))){
                    zacatek=1;
            }
        }
        //zkouska inverze
        else if ((nav_msg[0]&0x00000000000003FF)==0x000000000000029F){
            //na zacatku je synch. slovo
            if ((((nav_msg[3]&0xFC00000000000000)==(0x7C00000000000000))) && (((nav_msg[4])&(0x000000000000000F))==(0x000000000000000A))){
                    zacatek=1;
                    inverze=1;
            }
        }
        else{
            return -5;//neni zacatek stranky
        }
    if (zacatek==1){
        posun=0x0000000000000400; //maska
        k=0;
        for(i=239 ; i>-1 ; i--){
            if ((nav_msg[k]&posun)==0){
                if (inverze==0){
                    Prijato[i]=0;
                }
                else{
                    Prijato[i]=1;
                }
            }
            else{
                if (inverze==0){
                    Prijato[i]=1;
                }
                else{
                    Prijato[i]=0;
                }
            }
            posun=posun<<1;
            if (posun==0){ //jsme na konci radky
                posun=0x0000000000000001; //1 se vyhodila uz mimo, takze se da na zacatek
                k++; //a pokracujeme o radek vys
            }
        }
        Deinterleaving(Prijato);
        ViterbiDecoding(Prijato,Dekodovano);
        for( i=0 ; i<120 ; i++){
            Stranka[i+120]=Dekodovano[i];
        }
        if(Dekodovano[0]==1){ //pokud byla jako prvni dekodovana licha cast, tak o blok ''vys'' je suda (prvni) polovina dane stranky a v nasledujicim se dekoduje
            posun=0x0000000000000010; //pomocna promenna kterou se nasobi prijata posloupnos, abychom dostali jen ten jeden bit co chceme
            k=4;
            for(i=239 ; i>-1 ; i--){
                if ((nav_msg[k]&posun)==0){
                    if (inverze==0){
                        Prijato[i]=0;
                    }
                    else{
                        Prijato[i]=1;
                    }
                }
                else{
                    if (inverze==0){
                        Prijato[i]=1;
                    }
                    else{
                        Prijato[i]=0;
                    }
                }
                posun=posun<<1;
                if (posun==0){ //jsme na konci radky
                    posun=0x0000000000000001; //1 se vyhodila uz mimo, takze se da na zacaek
                    k++; //a pokracujeme o radek vys
                }
            }
            Deinterleaving(Prijato);
            ViterbiDecoding(Prijato,Dekodovano);
            for( i=0 ; i<120 ; i++ ){
                Stranka[i]=Dekodovano[i];
            }
        }
        else{  //posledni prijaty blok je suda tedy prvni cast
            return -2;
        }
        return 1;
    }
    return -3;
}

int CRC(uint8_t Page[]){
    uint8_t G_x[]={1,1,0,0,0,0,1,1,0,0,1,0,0,1,1,0,0,1,1,1,1,1,0,1,1};
    uint8_t m_x[220];
    uint8_t i,j;
    uint32_t Checksum=0;
    for( i=0 ; i<114 ; i++){
        m_x[i]=Page[i];
    }
    for ( i=120 ; i<226 ; i++){
        m_x[i-6]=Page[i];
    }

    for( i=0 ; i<196 ; i++){
        if(m_x[i]==0){
            continue;
        }
        for( j=0 ; j<25 ; j++){
            m_x[i+j]=m_x[i+j]^G_x[j];
        }
    }
    for( i=196 ; i<220 ; i++){
        Checksum=Checksum<<1;
        if(m_x[i]!=0){
            Checksum++;
        }
    }
    return Checksum;
}
int readvalue(uint8_t begin, uint8_t len, uint8_t page[], uint8_t sign){
    uint8_t i,beg;
    int val=0;
    int32_t pom=0;
    beg=begin;
    i=begin;
    if(sign==1){//sing=1 -> signed; sign=0 -> unsigned
        beg++;
        if(page[begin]==1){
            pom = pow(2,(len-1));
        }
    }
    for( i=beg ; i<(begin+len) ; i++ ){
        val=val<<1;
        if(i<114){ //protoze slovo je rozdeleno mezi sudou a lichou strankou a mezi castmi slova je tail a dalsi dva bity
            if(page[i]==1){
                val++;}
        }
        else{
            if(page[i+8]==1){
                val++;}
        }
    }
    return val-pom;
}
//cteni parametru a ukladani do promennych
void sortWT1(uint8_t page[],uint8_t SV_number){
    WordType1[SV_number-1].IODnav=readvalue(8,10,page,0);
    WordType1[SV_number-1].toe=readvalue(18,14,page,0);
    WordType1[SV_number-1].Mo=readvalue(32,32,page,1);
    WordType1[SV_number-1].e=readvalue(64,32,page,0);
    WordType1[SV_number-1].sqrtA=readvalue(96,32,page,0);
}
void sortWT2(uint8_t page[],uint8_t SV_number){
    WordType2[SV_number-1].IODnav=readvalue(8,10,page,0);
    WordType2[SV_number-1].OMEGA_0=readvalue(18,32,page,1);
    WordType2[SV_number-1].io=readvalue(50,32,page,1);
    WordType2[SV_number-1].omega=readvalue(82,32,page,1);
    WordType2[SV_number-1].ii=readvalue(114,14,page,1);
}
void sortWT3(uint8_t page[],uint8_t SV_number){
    WordType3[SV_number-1].IODnav=readvalue(8,10,page,0);
    WordType3[SV_number-1].omega_dot=readvalue(18,24,page,1);
    WordType3[SV_number-1].delta_n=readvalue(42,16,page,1);
    WordType3[SV_number-1].C_uc=readvalue(58,16,page,1);
    WordType3[SV_number-1].C_us=readvalue(74,16,page,1);
    WordType3[SV_number-1].C_rc=readvalue(90,16,page,1);
    WordType3[SV_number-1].C_rs=readvalue(106,16,page,1);
    WordType3[SV_number-1].SISA=readvalue(122,8,page,0);
}
void sortWT4(uint8_t page[],uint8_t SV_number){
    WordType4[SV_number-1].IODnav=readvalue(8,10,page,0);
    WordType4[SV_number-1].SVID=readvalue(18,6,page,0);
    WordType4[SV_number-1].C_ic=readvalue(24,16,page,1);
    WordType4[SV_number-1].C_is=readvalue(40,16,page,1);
    WordType4[SV_number-1].t_0c=readvalue(56,14,page,0);
    WordType4[SV_number-1].a_f0=readvalue(70,31,page,1);
    WordType4[SV_number-1].a_f1=readvalue(101,21,page,1);
    WordType4[SV_number-1].a_f2=readvalue(122,6,page,1);
}
void sortWT5(uint8_t page[],uint8_t SV_number){
    WordType5[SV_number-1].a_i0=readvalue(8,11,page,0);
    WordType5[SV_number-1].a_i1=readvalue(19,11,page,1);
    WordType5[SV_number-1].a_i2=readvalue(30,14,page,1);
    WordType5[SV_number-1].Region1=readvalue(44,1,page,0);
    WordType5[SV_number-1].Region2=readvalue(45,1,page,0);
    WordType5[SV_number-1].Region3=readvalue(46,1,page,0);
    WordType5[SV_number-1].Region4=readvalue(47,1,page,0);
    WordType5[SV_number-1].Region5=readvalue(48,1,page,0);
    WordType5[SV_number-1].BGDa=readvalue(49,10,page,1);
    WordType5[SV_number-1].BGDb=readvalue(59,10,page,1);
    WordType5[SV_number-1].E5b_Hs=readvalue(69,2,page,0);
    WordType5[SV_number-1].E1B_HS=readvalue(71,2,page,0);
    WordType5[SV_number-1].E5b_DVs=readvalue(73,1,page,0);
    WordType5[SV_number-1].E1B_DVs=readvalue(74,1,page,0);
    WordType5[SV_number-1].WN=readvalue(75,12,page,0);
    WordType5[SV_number-1].TOW=readvalue(87,20,page,0);
}
void sortWT6(uint8_t page[],uint8_t SV_number){
    WordType6[SV_number-1].A0=readvalue(8,32,page,1);
    WordType6[SV_number-1].A1=readvalue(40,24,page,1);
    WordType6[SV_number-1].deltat_LS=readvalue(64,8,page,1);
    WordType6[SV_number-1].t_ot=readvalue(72,8,page,0);
    WordType6[SV_number-1].WN_0t=readvalue(80,8,page,0);
    WordType6[SV_number-1].WN_LSF=readvalue(88,8,page,0);
    WordType6[SV_number-1].DN=readvalue(96,3,page,0);
    WordType6[SV_number-1].deltat_LSF=readvalue(99,8,page,1);
    WordType6[SV_number-1].TOW=readvalue(107,20,page,0);
}
void sortWT7(uint8_t page[],uint8_t SV_number){
    WordType7[SV_number-1].IODa=readvalue(8,4,page,0);
    WordType7[SV_number-1].WNa=readvalue(12,2,page,0);
    WordType7[SV_number-1].t_0a=readvalue(14,10,page,0);
    WordType7[SV_number-1].SVID1=readvalue(24,6,page,0);
    WordType7[SV_number-1].delta_sqrtA=readvalue(30,13,page,1);
    WordType7[SV_number-1].e=readvalue(43,11,page,0);
    WordType7[SV_number-1].omega=readvalue(54,16,page,1);
    WordType7[SV_number-1].d_i=readvalue(70,11,page,1);
    WordType7[SV_number-1].omega_0=readvalue(81,16,page,1);
    WordType7[SV_number-1].omega_dot=readvalue(97,11,page,1);
    WordType7[SV_number-1].M_0=readvalue(108,16,page,1);
}
void sortWT8(uint8_t page[],uint8_t SV_number){
    WordType8[SV_number-1].IODa=readvalue(8,4,page,0);
    WordType8[SV_number-1].a_f0=readvalue(12,16,page,1);
    WordType8[SV_number-1].a_f1=readvalue(28,13,page,1);
    WordType8[SV_number-1].E5b_HS=readvalue(41,2,page,0);
    WordType8[SV_number-1].E1B_HS=readvalue(43,2,page,0);
    WordType8[SV_number-1].SVID2=readvalue(45,6,page,0);
    WordType8[SV_number-1].delta_sqrtA=readvalue(51,13,page,1);
    WordType8[SV_number-1].e=readvalue(64,11,page,0);
    WordType8[SV_number-1].omega=readvalue(75,16,page,1);
    WordType8[SV_number-1].d_i=readvalue(91,11,page,1);
    WordType8[SV_number-1].omega_0=readvalue(102,16,page,1);
    WordType8[SV_number-1].omega_dot=readvalue(118,11,page,1);
}
void sortWT9(uint8_t page[],uint8_t SV_number){
    WordType9[SV_number-1].IODa=readvalue(8,4,page,0);
    WordType9[SV_number-1].WNa=readvalue(12,2,page,0);
    WordType9[SV_number-1].t_0a=readvalue(14,10,page,0);
    WordType9[SV_number-1].M_0=readvalue(24,16,page,1);
    WordType9[SV_number-1].a_f0=readvalue(40,16,page,1);
    WordType9[SV_number-1].a_f1=readvalue(56,13,page,1);
    WordType9[SV_number-1].E5b_HS=readvalue(69,2,page,0);
    WordType9[SV_number-1].E1B_HS=readvalue(71,2,page,0);
    WordType9[SV_number-1].SVID3=readvalue(73,6,page,0);
    WordType9[SV_number-1].delta_sqrtA=readvalue(79,13,page,1);
    WordType9[SV_number-1].e=readvalue(92,11,page,0);
    WordType9[SV_number-1].omega=readvalue(103,16,page,1);
    WordType9[SV_number-1].d_i=readvalue(119,11,page,1);
}
void sortWT10(uint8_t page[],uint8_t SV_number){
    WordType10[SV_number-1].IODa=readvalue(8,4,page,0);
    WordType10[SV_number-1].omega_0=readvalue(12,16,page,1);
    WordType10[SV_number-1].omega_dot=readvalue(28,11,page,1);
    WordType10[SV_number-1].M_0=readvalue(39,16,page,1);
    WordType10[SV_number-1].a_f0=readvalue(55,16,page,1);
    WordType10[SV_number-1].a_f1=readvalue(71,13,page,1);
    WordType10[SV_number-1].E5b_HS=readvalue(84,2,page,0);
    WordType10[SV_number-1].E1B_HS=readvalue(86,2,page,0);
    WordType10[SV_number-1].A_0G=readvalue(88,16,page,1);
    WordType10[SV_number-1].A_1G=readvalue(104,12,page,1);
    WordType10[SV_number-1].t_0G=readvalue(116,8,page,0);
    WordType10[SV_number-1].WN_0G=readvalue(124,6,page,0);
}
void sortWT0(uint8_t page[],uint8_t SV_number){
    WordType0[SV_number-1].Time=readvalue(8,2,page,0);
    WordType0[SV_number-1].WN=readvalue(98,12,page,0);
    WordType0[SV_number-1].TOW=readvalue(110,20,page,0);
}