/* ----------------------------------------------------------
        Routinen zum Empfang der NMEA Nachrichten und deren
        Auswertung

        $GNRMC,080947.00,A,5200.19391,N,00910.23188,E,30.585,231.29,180121,,,A*45

---------------------------------------------------------- */

#include "gps.h"
#include "cities.h"

/* Indiziert einen NMEA Teststring in den Buffer und simuliert damit
den einwandfreien Empfang */

#define BAUDRATE    9600
#define BUFSIZE     100

/* -------------------- Interface Variablen  --------------------- */

static nmea_info_t GPS;        /* PRIVAT:  Datenregister */

/* Interface Read Only auf die Elemente des Basis Structs */
const nmea_info_t* roGPS = (const nmea_info_t*)&GPS;

volatile _Bool fGPSValidFix  = false;       /* 1: Gültiger Fix vorhanden */
volatile _Bool fNewDataAvailable = false;   /* Neue Daten im Struct vorhanden */
volatile _Bool fNewNMEAToParse = false;

/* -------------------- Private Variablen  --------------------- */

static char NMEAStrBuf[2][BUFSIZE];         /* 2 Buffer, abwechselnd beschrieben */
static int gps_buf_idx = 0;                 /* Zeiger auf aktiven Buffer */
static int NmeaBuf_Ptr = 0;

/* Extern Interface */
char *BufToRead = NMEAStrBuf[0];

/* ------ Interface zu externen Funktionen --------*/

/* Stellt die RTc einmalig nach GPS */
uint8_t GPS_SetGPSToRTC() {

    LEDStatus[ROT] =  OFF;
    LEDStatus[GRUEN] = OFF;
    LEDStatus[BLAU] = BLINK;

    while (!roGPS->fGPSDataValid) {
       /* Liegt ein neuer NMEA Satz vor ? */
       if (fNewNMEAToParse) {
           fNewNMEAToParse = false;
           Parse_NMEAToStruct(BufToRead);
       }
    }

    uint32_t unixtime = unixzeit(roGPS->time.tm_year,
                                 roGPS->time.tm_mon,
                                 roGPS->time.tm_mday,
                                 roGPS->time.tm_hour,
                                 roGPS->time.tm_min,
                                 roGPS->time.tm_sec);
    /* RTC neu setzen */
    Set_NewTime(unixtime);
    return SUCCESS;
}

/* Initialisiert die UART1 und stellt Verbindung zur ISR her */
void UART1_Init()
{
    USART_InitTypeDef usart1_init_struct;
    GPIO_InitTypeDef gpioa_init_struct;
    NVIC_InitTypeDef NVIC_InitStructure;

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1 | RCC_APB2Periph_AFIO |
                           RCC_APB2Periph_GPIOA, ENABLE);

    USART_DeInit(USART1);

    /* GPIOA PA9(TX) / PA10(RX) alternative function */
    gpioa_init_struct.GPIO_Pin = GPIO_Pin_9;
    gpioa_init_struct.GPIO_Speed = GPIO_Speed_50MHz;
    gpioa_init_struct.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_Init(GPIOA, &gpioa_init_struct);

    gpioa_init_struct.GPIO_Pin = GPIO_Pin_10;
    gpioa_init_struct.GPIO_Speed = GPIO_Speed_50MHz;
    gpioa_init_struct.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_Init(GPIOA, &gpioa_init_struct);

    /* Baud rate 9600, 8-bit data, One stop bit
     * No parity, Tx, No HW flow control */
    usart1_init_struct.USART_BaudRate   = 9600;
    usart1_init_struct.USART_WordLength = USART_WordLength_8b;
    usart1_init_struct.USART_StopBits   = USART_StopBits_1;
    usart1_init_struct.USART_Parity     = USART_Parity_No ;
    usart1_init_struct.USART_Mode       = USART_Mode_Rx | USART_Mode_Tx;
    usart1_init_struct.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
    USART_Init(USART1, &usart1_init_struct);

    /* Interruptanbindung an den NVIC */
    NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;  /* Höchste Prio */
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    /* Pending Ints löschen */
    ClearPendingIT();

    /* Ints aktivieren */
    USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);       /* RXNE Interrupt */
    USART_ITConfig(USART1, USART_IT_IDLE, ENABLE);       /* RX Idle Interrupt */

    USART_Cmd(USART1, ENABLE);
    NVIC_EnableIRQ(USART1_IRQn);
}

/* Schaltet GPS Enable ein/aus + Init */
void GPS_Power(FunctionalState status)
{
    static uint8_t merker = 0;          /* Doppeldurchläufe vermeiden */
    GPIO_InitTypeDef  GPIO_InitStructure;

    GPIO_InitStructure.GPIO_Pin   = GPS_ENABLE_PIN;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;

    __disable_irq();

    switch (status)
    {
    case ENABLE:
        if (merker != 1)
        {
            GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_IN_FLOATING;
            GPIO_Init(GPS_ENABLE_PORT, &GPIO_InitStructure);
            /* USART und ISR einschalten */
            USART_Cmd(USART1, ENABLE);
            ClearPendingIT();
            NVIC_EnableIRQ(USART1_IRQn);
            merker = 1;
        }
        break;
    case DISABLE:
        if (merker != 2)
        {
            /* USART1 und ISR abschalten */
            NVIC_DisableIRQ(USART1_IRQn);
            USART_Cmd(USART1, DISABLE);
            GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_Out_PP;
            GPIO_Init(GPS_ENABLE_PORT, &GPIO_InitStructure);
            GPIO_ResetBits(GPS_ENABLE_PORT, GPS_ENABLE_PIN);
            /* Flags ungültig setzen */
            fGPSValidFix = false;
            merker = 2;
        }
        break;
    }

    __enable_irq();
}


/* ----------------------------------------------------
    UART1 Interrupt Service Routine
----------------------------------------------------- */

void USART1_IRQHandler()
{
    /* Neues Byte wurde empfangen vom GPS Modul */
    if (USART_GetITStatus(USART1, USART_IT_RXNE) == SET)
    {
        char val = USART1->DR;
        /* Wenn $ dann beginnt neuer NMEA String */
        if (val=='$')
            NmeaBuf_Ptr = 0;
        NMEAStrBuf[gps_buf_idx][NmeaBuf_Ptr] = val;
        /* Pointer auf nächstes Byte */
        NmeaBuf_Ptr = (NmeaBuf_Ptr + 1) % BUFSIZE;
        /* String terminieren !!! */
        NMEAStrBuf[gps_buf_idx][NmeaBuf_Ptr] = 0;
        SetLED(BOARD,ENABLE);
    }

    /* ------------------------------------------------------- */

    /* Timeout, letztes Byte wurde empfangen */
    if (USART_GetITStatus(USART1, USART_IT_IDLE) == SET)
    {
        /* FIXXXXXME Test */
        #ifdef TESTSTRING
            //strcpy(NMEAStrBuf[gps_buf_idx],"$GNRMC,174154.00,A,5200.56286,N,00910.04357,E,28.038,155.49,280421,,,A*47\r\n");
            strcpy(NMEAStrBuf[gps_buf_idx],"$GNRMC,113230.00,A,5159.42256,N,00907.00730,E,0.000,289.00,020621,,,A*71\r\n");

        #endif
        NmeaBuf_Ptr = 0;                        /* Reset RX Pointer */
        BufToRead = NMEAStrBuf[gps_buf_idx];    /* Readbuffer aktivieren */
        gps_buf_idx = !gps_buf_idx;             /* Anderen Buffer wählen */
        /* Flags setzen */
        fNewNMEAToParse = true;
        f_ISRAllowWakeup = true;
    }

    /* Alle Error Flags löschen */
    ClearPendingIT();
}

/* Umrechnung von Geo Koordinaten in Plus Code mit 12 Stellen */

const char pcode[21] = {'2','3','4','5','6','7','8','9','C',        \
                        'F','G','H','J','M','P','Q','R','V','W','X'};

uint8_t GPS_CalcPlusCode(double gps_lat,double gps_lon, char* pluscode)
{
    char code[16];
    int p = 0;

    /* Eingangswerte positiv machen */
    if ((gps_lat = gps_lat + 90) > 360)
        return ERROR;
    if ((gps_lon = gps_lon + 180) > 360)
         return ERROR;

    for (int i = 0; i < 5; i++)
    {
       int mlat = ((int)(gps_lat/20)) % 20;     // % 20 sicherheitshalber
       int mlon = ((int)(gps_lon/20)) % 20;

       code[p++] = pcode[mlat];
       code[p++] = pcode[mlon];

       /* Reste berechnen und x 20  */
       gps_lat = (gps_lat-(mlat*20))*20;
       gps_lon = (gps_lon-(mlon*20))*20;

       /* Plus Zeichen einfügen nach 6 Stellen */
       if (i==3)
           code[p++] = '+';
    }
    /* String terminieren, letzte lon Stelle killen */
    code [p] = '\0';
    strcpy(pluscode,code);

    return SUCCESS;
}


/* Berechnet zyklisch den Abstand der aktuellen Position zur nächst
   nahen Grossstadt. 100 Staedte sind gespeichert, Pos. 0 - 99
*/

/* Aufruf GPS_CalcCity(lon,lat,&stadt,&DistToHome); */

uint8_t GPS_CalcCity(double gps_lat, double gps_lon, double* tlat, double* tlon, const char** stadt)
{
    static int citynow    = 0;
    static int stadt_nr   = 0;
    static float abstand  = 999.0;

    /* Suche Stadt mit dem kleinsten Abstand zur aktuellen Position */
    float foo = CalcDistance(gps_lat,gps_lon,citys[citynow].lat,citys[citynow].lon);
    if (foo < abstand) {
        abstand = foo;
        stadt_nr = citynow;
    }

    /* Bei jedem Durchlauf wird nur eine Berechnung gemacht */
    citynow = (citynow + 1) % NOCITYS;

    /* Alle durchsucht worden ? */
    if (!citynow) {
        *stadt =  citys[stadt_nr].name;
        *tlat = citys[stadt_nr].lat;
        *tlon = citys[stadt_nr].lon;
        abstand = 9999;
        return SUCCESS;
    }

    return ERROR;
}

/* Zerlegt den NMEA String in den Struct */
uint8_t Parse_NMEAToStruct(char* Buf)
{
    #define MAX_NMEA_FIELD     16
    char* nmea_fields[MAX_NMEA_FIELD];

/* ---------------- Überprüfe die ersten Zeichen auf $GNRMC -------------*/

    /* Alles ungültig markieren */
    fGPSValidFix = false;
    GPS.fGPSDataValid = false;

    const char str1[] = "$GNRMC";

    /* RMC Kürzel vorhanden? */
    if (!strncmp(Buf,str1,sizeof(str1)-1) == 0)
        return ERROR;

    /* Stern vor prüfsumme suchen */
    char* k = Buf;
    uint8_t ilen = 0;
    while ((*k++ != '*') && (ilen++ < strlen(Buf)));    /* Suchen... */

    /* Prüfsumme extrahieren als int */
    char foo[8] = {0,0,0,0,0,0,0,0};
#ifndef TESTSTRING
    strncpy(foo,k,2);
    uint32_t sum = hex2int(foo);
    // uint8_t sum = (int)strtol(strncpy(foo,k,2), NULL, 16);

    /* Prüfsumme des Nmea Strings bilden */
    uint32_t xor = 0;
    for (uint8_t i = 1; i < ilen; i++)
        xor ^= Buf[i];

    /* Falsche Prüfsumme, dann raus */
    if (sum != xor) {
        return ERROR;
    }
#endif

    /* String in SD Card Buffer kopieren mit seiner Size*/
    strcpy(GPS.NmeaStr,Buf);

    /* String jetzt zerteilen, der durch , separiert ist */
    k = Buf;
    uint8_t i = 0;
    nmea_fields[i++] = k;  /* Erstes Element setzen, NMEA Typ RMC */

    /* Vektor Feld auf einzelne Teil Strings erzeugen */
    while (((k = strchr(k,',')) != NULL) && (i < MAX_NMEA_FIELD))
    {
        *k = '\0';
        nmea_fields[i++] = ++k;
    }

    /* Gültiger Fix = 1, kein Fix = 0 */
    if (strncmp(nmea_fields[2],"A",1) == 0)
    {
        /* -------------------------------------------------- */
        fGPSValidFix = true;

        /* Extrahiere die Daten aus dem NMEA String */

        /* ---- Uhrzeit extrahieren ---- */
        memset(foo,0,sizeof(foo));   /* 0 terminieren */
        char *p = nmea_fields[1];

        if (*p != '\0')
        {
            /* Sind nur Zahlen im Feld? */
            for (int i=0; i<6;i++) {
                if (!isdigit((int)(*(p+i))))
                    return ERROR;
            }

            /* Strings wandeln */
            uint8_t h = atoi(strncpy(foo,p,2));
            p +=2;
            uint8_t m = atoi(strncpy(foo,p,2));
            p +=2;
            uint8_t s = atoi(strncpy(foo,p,2));

            /* Plausibilitätsprüfung */
            if ((h>23) || (m>59) || (s>59))
                return ERROR;

            GPS.time.tm_hour = h;
            GPS.time.tm_min  = m;
            GPS.time.tm_sec  = s;

        } else
            return ERROR;

        /* ---- Latitude (Breite) 5200.38115 ---- */
        char grad[4];
        char minuten[10];
        memset(grad,0,sizeof(grad));
        memset(minuten,0,sizeof(minuten));

        p = nmea_fields[3];
        if (*p != '\0')
        {
            /* Strings extrahieren */
            strncpy(grad,p,2);
            strncpy(minuten,p+2,8);
            /* In DezGrad Float wandeln */
            GPS.latitude = atof(grad) + atof(minuten)/60;
            /* DezGrad in String wandeln */
            ftoa(GPS.latitude,GPS.latstr,5);
        } else
            return ERROR;

        /* ---- Longtitude (Laenge) 00910.09884 ---- */
        p = nmea_fields[5];
        memset(grad,0,sizeof(grad));
        memset(minuten,0,sizeof(minuten));

        if (*p != '\0')
        {
            /* Strings extrahieren */
            strncpy(grad,p,3);
            strncpy(minuten,p+3,8);
            /* In DezGrad Float wandeln */
            GPS.longtitude = atof(grad) + (atof(minuten)/60);
            /* DezGrad in String wandeln */
            ftoa(GPS.longtitude,GPS.longstr,5);
        } else
            return ERROR;

        /* ---- Geschwindigkeit in kmh ---- */
        p = nmea_fields[7];
        GPS.speedkmh = 0;
        if (*p != '\0') {
            GPS.speedkmh = (double)atoi(p) * 1.852;
        } else
            return ERROR;

//        /* ---- Kompass Richtung 0...360 bzgl Norden ---- */
//        p = nmea_fields[8];
//        memset(foo,0,sizeof(foo));
//        GPS.heading = -1;    /* ungültig markieren */
//        if (*p != '\0') {
//            /* Auswertung der Fahrtrichtung. Diese stellt sich erst in Bewewgung ein! */
//            GPS.heading = (atoi(p) < 360) ? atoi(p) : 0;
//        }

        /* --- Datum DDMMYY ---- */
        p = nmea_fields[9];
        memset(foo,0,sizeof(foo));
        if (*p != '\0')
        {
            /* Sind nur Zahlen im Feld? */
            for (int i=0; i<6;i++) {
                if (!isdigit((int)(*(p+i))))
                    return ERROR;
            }

            /* Strings umwandeln */
            int day = atoi(strncpy(foo,p,2));
            p +=2;
            int mon = atoi(strncpy(foo,p,2));
            p +=2;
            int yr  = atoi(strncpy(foo,p,2));

            /* Plausibilitätsprüfung */
            if ((yr<21) || (mon>12) || (day>31))
                return ERROR;

            GPS.time.tm_year = yr + 2000;
            GPS.time.tm_mon = mon;
            GPS.time.tm_mday = day;
         } else
            return ERROR;

        /* Der Datensatz war gültig */
        GPS_CalcPlusCode(GPS.latitude,GPS.longtitude,GPS.pluscode);

        GPS.fGPSDataValid = true;   /* Beide Info Flags setzen */
        fNewDataAvailable = true;
}


   return SUCCESS;
}

/*
Stop Modul:
B5 62 06 57 08 00 01 00 00 00 50 4F 54 53 AC 85

Start Modul:
B5 62 06 57 08 00 01 00 00 00 20 4E 55 52 7B C3

*/

/* Flag Verhindert mehrfaches Schalten */
_Bool fGPS_OnSwitch = true;

/* Stoppt das GPS Modul */
void GPS_StopGNSS()
{
    const uint8_t stopcmd[16] = {0xB5,0x62,0x06,0x57,0x08,0x00,0x01,0x00,
                                 0x00,0x00,0x50,0x4F,0x54,0x53,0xAC,0x85
                                };
    __disable_irq();
    if (fGPS_OnSwitch == true)
    {
        for (uint8_t i = 0; i < sizeof(stopcmd); i++)
        {
            USART_SendData(USART1,stopcmd[i]);
            while (!USART_GetFlagStatus(USART1,USART_FLAG_TXE));
        }
        NVIC_DisableIRQ(USART1_IRQn);
        USART_Cmd(USART1, DISABLE);
        fGPS_OnSwitch = false;
    }
    __enable_irq();
}

/* Neustart des GPS Moduls */
void GPS_StartGNSS()
{
    const uint8_t startcmd[16] = {0xB5,0x62,0x06,0x57,0x08,0x00,0x01,0x00,
                                  0x00,0x00,0x20,0x4E,0x55,0x52,0x7B,0xC3
                                 };
    __disable_irq();
    if (fGPS_OnSwitch == false)
    {
        USART_Cmd(USART1, ENABLE);
        for (uint8_t i = 0; i < sizeof(startcmd); i++)
        {
            USART_SendData(USART1,startcmd[i]);
            while (!USART_GetFlagStatus(USART1,USART_FLAG_TXE));
        }
        NVIC_EnableIRQ(USART1_IRQn);
        fGPS_OnSwitch = true;
    }
    __enable_irq();
}