//
// Created by YY on 2019/12/23.
//

#include <cstring>
#include <cstdio>
#include <pthread.h>
#include <cmath>
#include <cstdlib>
#include <cctype>
#include "rtk.h"
#include "parse_gps.h"
#include "../common/serial_port.h"
#include "../jni_log.h"
#include "../driver_test.h"
#include "../utils/num.h"
#include "../defs.h"
#include "../common/apptimer.h"
#include "../rtk_platform/platform.h"
#include "../native-lib.h"

#define DEBUG(fmt, args...)     LOGD("<rtk> <%s>: " fmt, __func__, ##args)

#define RTK_MODULE_UART         UART_0

#define PARSE_BUFF_SIZE         4096

const static char FACTORY[] = "FRESET\r\n";
const static char REBOOT[] = "RESET\r\n";
const static char INQ_PJK_PARAM[] = "LOG PJKPARA\r\n";
const static char AY_PJKPARAM[] = "set pjkpara 6378137 298.257223563 29.51245330924 106.4553361945 0 0\r\n";
const static char UNLOGALL[] = "unlogall\r\n";
const static char IFCOM2[] = "interfacemode com2 auto auto on\r\n";
const static char SAVECONFIG[] = "saveconfig\r\n";
const static char *PJKITEMS[] = {"gptra", "ptnlpjk"};
const static char *GPSITEMS[] = {"gpgga", "gprmc", "gpvtg"};

static gpsStatus_t gpsStatus;

static rtk_info CurrRTKInfo;
static bool needSetPjk = false;
static void CheckPjkParam(void);
static void CheckPjkParamTimeout(union sigval sig);

static void *UartThread(void *p);

void ConfigRTKModule(void)
{
    // TODO
    DEBUG("ConfigRTKModule");

    memset(&CurrRTKInfo, 0, sizeof(CurrRTKInfo));
    CurrRTKInfo.hh = -1;

    memset(&gpsStatus, 0, sizeof(gpsStatus));
    gpsStatus.hh = -1;

    static struct serial_config serialConfig;

    strcpy(serialConfig.name, "/dev/ttyHSL0");
    serialConfig.baud = 115200;
    serialConfig.data_bit = 8;
    serialConfig.verify_bit = 'N';
    serialConfig.stop_bit = 1;
    serialConfig.flow_ctrl = 0;

    pthread_t pid;
    pthread_attr_t attr;
    pthread_attr_init(&attr);
    pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);//detached
    pthread_create(&pid, &attr, UartThread, &serialConfig);
}

void FactorySettings(void)
{
    WriteSerialPort(RTK_MODULE_UART, FACTORY, strlen(FACTORY));
}

void RebootModule(void)
{
    WriteSerialPort(RTK_MODULE_UART, REBOOT, strlen(REBOOT));
}

void handleRTKRebootComp(const struct nmea *s)
{
    DEBUG("RTK Reboot complete!!");
    CheckPjkParam();
}

void handlePJKParam(const struct nmea *s) {
//PJK Parameter: A:6378137.000, 1/F:298.257223563, B0:0.000000deg, L0:120.000000, N0:0.000, E0:500000.000.
//PJK Parameter: A:6378137.000, 1/F:298.257223563, B0:29.512453deg, L0:106.455336, N0:0.000, E0:0.000.
//    bool setparam = true;
//
//    const char DP1[] = "A:6378137.000";
//    const char DP2[] = "1/F:298.257223563";
//    const char DP3[] = "B0:29.512453deg";
//    const char DP4[] = "L0:106.455336";
//    const char DP5[] = "N0:0.000";
//    const char DP6[] = "E0:0.000";

    AppTimer_delete(CheckPjkParamTimeout);

//    for (int i = 0; i < s->nmea_num; ++i) {
//        char out[64] = {0};
//
//        memcpy(out, s->nmea_value[i].data, s->nmea_value[i].length);
//
//        DEBUG("handlePJKParam = %s", out);
//    }
//
//    if (s->nmea_num != 6) return;
//
//    if (memcmp(s->nmea_value[0].data, DP1, strlen(DP1))) {
//        setparam = true;
//    }
//    if (memcmp(s->nmea_value[1].data, DP2, strlen(DP2))) {
//        setparam = true;
//    }
//    if (memcmp(s->nmea_value[2].data, DP3, strlen(DP3))) {
//        setparam = true;
//    }
//    if (memcmp(s->nmea_value[3].data, DP4, strlen(DP4))) {
//        setparam = true;
//    }
//    if (memcmp(s->nmea_value[4].data, DP5, strlen(DP5))) {
//        setparam = true;
//    }
//    if (memcmp(s->nmea_value[5].data, DP6, strlen(DP6))) {
//        setparam = true;
//    }
//
//    if (setparam) {
//        SetAYFactoryParam(5);
//    }

    SetAYFactoryParam(5);
    needSetPjk = true;
}

void SetAYFactoryParam(int freq)
{
    WriteSerialPort(RTK_MODULE_UART, UNLOGALL, strlen(UNLOGALL));
    WriteSerialPort(RTK_MODULE_UART, IFCOM2, strlen(IFCOM2));

    if (freq == 0)
        freq = 5;

    for (int i = 0; i < sizeof(PJKITEMS)/ sizeof(PJKITEMS[0]); ++i) {
        char cmd[64];
        sprintf(cmd, "log com1 %s ontime %0.1f\r\n", PJKITEMS[i], 1.0/(double)freq);
        WriteSerialPort(RTK_MODULE_UART, cmd, strlen(cmd));
    }

    for (int i = 0; i <  sizeof(GPSITEMS)/ sizeof(GPSITEMS[0]); ++i) {
        char cmd[64];
        sprintf(cmd, "log com1 %s ontime %0.1f\r\n", GPSITEMS[i], 1.0/(double)freq);
        WriteSerialPort(RTK_MODULE_UART, cmd, strlen(cmd));
    }

//    WriteSerialPort(RTK_MODULE_UART, AY_PJKPARAM, strlen(AY_PJKPARAM));
//    WriteSerialPort(RTK_MODULE_UART, SAVECONFIG, strlen(SAVECONFIG));
}

void SetPjkPara(int centLon)
{
    char buff[64];

    sprintf(buff, "set pjkpara 6378137 298.257223563 0 %d 0 500000\r\n", centLon);

    WriteSerialPort(RTK_MODULE_UART, buff, strlen(buff));

    DEBUG("%s", buff);
}

void GetGpsStatus(gpsStatus_t &data)
{
    data = gpsStatus;
}

static void *UartThread(void *p) {
    struct serial_config *cfg = (struct serial_config *) p;

    int res = InitSerialPort(RTK_MODULE_UART, cfg->baud, cfg->data_bit, cfg->verify_bit, cfg->stop_bit, cfg->flow_ctrl);
    DEBUG("Serial %s open %d", cfg->name, res);
    uint8_t RxBuf[PARSE_BUFF_SIZE];
    int RxBufLen = 0;

    if (res == 0) {
        CheckPjkParam();
    }

    while (res == 0) {
        int ul = ReadSerialPort(RTK_MODULE_UART, (uint8_t *)RxBuf + RxBufLen, sizeof(RxBuf) - RxBufLen);
        RxBufLen += ul;

        /*{
            static char buffd[16384];

            buffd[0] = 0;
            int i = 0;
            for (i = 0; i < ul; i++) {
                if ((i % 32) == 0) {
                    sprintf(buffd + strlen(buffd), "\n");
                }
                sprintf(buffd + strlen(buffd), "%02X ", RxBuf[i]);
                if (strlen(buffd) > 800) {
                    DEBUG("%s <- %s...", "UART", buffd);
                    buffd[0] = 0;
                }
            }
            if (strlen(buffd) > 0)
                DEBUG("%s <- %s", "UART", buffd);
        }*/

        if (RxBufLen > 0) {
#if 1
            const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
            if(ptr != RxBuf) {
                memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));
                RxBufLen -= ptr - RxBuf;
            } else if(RxBufLen == PARSE_BUFF_SIZE) {        //填满了，且没有一个\r，都抛弃
                DEBUG("Parse GPS error");
                RxBufLen = 0;
            }
#else
            RxBufLen = 0;           //PC模拟用时
#endif
        }
    }
    if (res == 0) {
        UninitSerialPort(RTK_MODULE_UART);
    }
    pthread_exit(NULL);
}


void handleUnrecognisedNMEA(const uint8_t *data, uint16_t length) {
//    char buff[4096] = {0};
//    memcpy(buff, data, MIN(length, 3000));
//    DEBUG("handleUnrecognisedNMEA: %s", buff);
}

void handleGPGGA(const struct nmea *s)
{
    static uint32_t qfCnt = 0;

    DEBUG("handleGPGGA num = %d", s->nmea_num);
    if (s->nmea_num >= 10) {
        gpsStatus.gps_status = str2int(s->nmea_value[5].data, s->nmea_value[5].length);

        int hh = str2int(s->nmea_value[0].data, 2);
        int mm = str2int(s->nmea_value[0].data + 2, 2);
        int ss = str2int(s->nmea_value[0].data + 4, 2);
        int mss = str2int(s->nmea_value[0].data + 7, 2);

        double lat1, lat2, lon1, lon2, alt;
        str2float(&lat1, s->nmea_value[1].data, 2);
        str2float(&lat2, s->nmea_value[1].data+2, s->nmea_value[1].length-2);

        str2float(&lon1, s->nmea_value[3].data, 3);
        str2float(&lon2, s->nmea_value[3].data+3, s->nmea_value[3].length-3);

        lat1 = lat1 + lat2/60.0;
        lon1 = lon1 + lon2/60.0;

        str2float(&alt, s->nmea_value[8].data, s->nmea_value[8].length);

        gpsStatus.latitude = lat1;
        gpsStatus.longitude = lon1;
        gpsStatus.altitude = alt;
        gpsStatus.satNum = str2int(s->nmea_value[6].data, s->nmea_value[6].length);

        if (hh == gpsStatus.hh && mm == gpsStatus.mm && ss == gpsStatus.ss && mss == gpsStatus.mss) {
            // 同步的RMC消息也收集了
            PlatformStatusChanged(GPS_UPDATE_EVT, (uint8_t *)&gpsStatus, sizeof(gpsStatus));
        }
        gpsStatus.hh = hh;
        gpsStatus.mm = mm;
        gpsStatus.ss = ss;
        gpsStatus.mss = mss;

        // 计算中央子午线
        int qf = str2int(s->nmea_value[5].data, s->nmea_value[5].length);
        if (qf > 0) {
            qfCnt++;
            if (needSetPjk && qfCnt >= 3) {
                needSetPjk = false;

                DEBUG("set pjkpara 原始经度 %f", lon1);
                SetPjkPara(((int) (lon1 / 3.0 + 0.5)) * 3);
            }
        } else {
            qfCnt = 0;
        }
    }
}

void handleGPGSA(const struct nmea *s) {
}

void handleGPGSV(const struct nmea *s) {
}

void handleGPRMC(const struct nmea *s)
{
    DEBUG("handleGPRMC num = %d", s->nmea_num);

    if (s->nmea_num >= 9) {
        int hh = str2int(s->nmea_value[0].data, 2);
        int mm = str2int(s->nmea_value[0].data + 2, 2);
        int ss = str2int(s->nmea_value[0].data + 4, 2);
        int mss = str2int(s->nmea_value[0].data + 7, 2);

        gpsStatus.DD = str2int(s->nmea_value[8].data, 2);
        gpsStatus.MM = str2int(s->nmea_value[8].data + 2, 2);
        gpsStatus.YY = str2int(s->nmea_value[8].data + 4, 2);

        double speed;

        str2float(&speed, s->nmea_value[6].data, s->nmea_value[6].length);

        gpsStatus.speed = speed * 1.85184;          // 节 -> 公里

        double trackTure;
        str2float(&trackTure, s->nmea_value[7].data, s->nmea_value[7].length);
        gpsStatus.trackTure = trackTure;

        if (hh == gpsStatus.hh && mm == gpsStatus.mm && ss == gpsStatus.ss && mss == gpsStatus.mss) {
            // 同步的GGA消息也收集了
            PlatformStatusChanged(GPS_UPDATE_EVT, (uint8_t *)&gpsStatus, sizeof(gpsStatus));
        }
        gpsStatus.hh = hh;
        gpsStatus.mm = mm;
        gpsStatus.ss = ss;
        gpsStatus.mss = mss;
    }
}

const char *GPS_SOL_OK = "SOL_COMPUTED";
const char *GPS_SOL_OBS = "INSUFFICIENT_OBS";
const char *GPS_SOL_CS = "COLD_START";

const char *GPS_POS_NONE = "NONE";
const char *GPS_POS_FIX = "FIXEDPOS";
const char *GPS_POS_SIG = "SINGLE";
const char *GPS_POS_PDIFF = "PSRDIFF";
const char *GPS_POS_NARF = "NARROW_FLOAT";
const char *GPS_POS_WIDI = "WIDE_INT";
const char *GPS_POS_NARI = "NARROW_INT";
const char *GPS_POS_SWIDL = "SUPER WIDE-LANE";

void handleBESTPOSA(const struct nmea *s) {
    DEBUG("handleBESTPOSA num = %d", s->nmea_num);
    double lat, lon;

    if (memcmp(s->nmea_value[0].data, GPS_SOL_OK, s->nmea_value[0].length)) {
        str2float(&lat, s->nmea_value[2].data, s->nmea_value[2].length);
        str2float(&lon, s->nmea_value[3].data, s->nmea_value[3].length);
    }
}

void handleGPVTG(const struct nmea *s) {
}

void handleGPGLL(const struct nmea *s) {
}

void handleGPZDA(const struct nmea *s) {
}

void handlePJK(const struct nmea *s) {
    DEBUG("handlePJK num = %d", s->nmea_num);

    int hh = str2int(s->nmea_value[0].data, 2);
    int mm = str2int(s->nmea_value[0].data + 2, 2);
    int ss = str2int(s->nmea_value[0].data + 4, 2);
    int dss = str2int(s->nmea_value[0].data + 7, 2);

    CurrRTKInfo.MM = str2int(s->nmea_value[1].data, 2);
    CurrRTKInfo.DD = str2int(s->nmea_value[1].data + 2, 2);
    CurrRTKInfo.YY = str2int(s->nmea_value[1].data + 4, 2);

    CurrRTKInfo.qf = str2int(s->nmea_value[6].data, s->nmea_value[6].length);

    // NOTE: RTK模块是以南北向为X轴，西东向为Y轴，我们交换下，以符合一般逻辑
    str2float(&CurrRTKInfo.y, s->nmea_value[2].data, s->nmea_value[2].length);
    str2float(&CurrRTKInfo.x, s->nmea_value[4].data, s->nmea_value[4].length);

    static double sbx = -12;
    static double sby = 27;

//    CurrRTKInfo.x = (-2.8984 - 4.913)/2;
//    CurrRTKInfo.y = (31.6962 + 29.4974)/2;

    sbx += 0.01;
    sby += 0.01;

//    const double by1 = 28.013;
//    const double bx1 = -11.9669;
//
//    const double by2 = 29.3232;
//    const double bx2 = -9.5057;
//
//    static double xx = -10.9669, yy = 28.013;
//
//    CurrRTKInfo.y = yy;
//    CurrRTKInfo.x = xx;
//
//    if (forwardx) {
//        xx += 0.02;
//        yy += 0.02 * (by2 - by1) / (bx2 - bx1);
//    } else {
//        xx -= 0.02;
//        yy -= 0.02 * (by2 - by1) / (bx2 - bx1);
//    }

    if (CurrRTKInfo.hh == hh && CurrRTKInfo.mm == mm && CurrRTKInfo.ss == ss && CurrRTKInfo.dss == dss) {
        PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&CurrRTKInfo, sizeof(CurrRTKInfo));
//        UpdateRTKInfo(&CurrRTKInfo);
//        up_num++;
        /*if ((up_num % 5) == 0)*/ {
//            NewMgrEvent(DRIVER_UPDATE_EVT);
        }
    }
    CurrRTKInfo.hh = hh;
    CurrRTKInfo.mm = mm;
    CurrRTKInfo.ss = ss;
    CurrRTKInfo.dss = dss;
}

void handleGPTRA(const struct nmea *s) {
    DEBUG("handleGPTRA num = %d", s->nmea_num);

    int hh = str2int(s->nmea_value[0].data, 2);
    int mm = str2int(s->nmea_value[0].data + 2, 2);
    int ss = str2int(s->nmea_value[0].data + 4, 2);
    int dss = str2int(s->nmea_value[0].data + 7, 2);

    str2float(&CurrRTKInfo.heading, s->nmea_value[1].data, s->nmea_value[1].length);
    str2float(&CurrRTKInfo.pitch, s->nmea_value[2].data, s->nmea_value[2].length);
    str2float(&CurrRTKInfo.roll, s->nmea_value[3].data, s->nmea_value[3].length);

//    CurrRTKInfo.heading = 60;

    static double deg = 0;
//    CurrRTKInfo.heading = deg;
    deg += 2.0;
    if (deg >= 360) deg = 0;

    if (CurrRTKInfo.hh == hh && CurrRTKInfo.mm == mm && CurrRTKInfo.ss == ss && CurrRTKInfo.dss == dss) {
        PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&CurrRTKInfo, sizeof(CurrRTKInfo));
//        UpdateRTKInfo(&CurrRTKInfo);
//        up_num++;
        /*if ((up_num % 5) == 0)*/ {
//            NewMgrEvent(DRIVER_UPDATE_EVT);
        }
    }
    CurrRTKInfo.hh = hh;
    CurrRTKInfo.mm = mm;
    CurrRTKInfo.ss = ss;
    CurrRTKInfo.dss = dss;
}

static void CheckPjkParam(void)
{
    int n = WriteSerialPort(RTK_MODULE_UART, INQ_PJK_PARAM, strlen(INQ_PJK_PARAM));
    DEBUG("CN = %d", n);
    AppTimer_delete(CheckPjkParamTimeout);
    AppTimer_add(CheckPjkParamTimeout, D_SEC(3));
}

static void CheckPjkParamTimeout(union sigval sig) {
    AppTimer_delete(CheckPjkParamTimeout);

    DEBUG("RTK Module failure!!");

//    PlayTTS("RTK模块无法通讯", NULL);

    CheckPjkParam();
}