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

#include <cstring>
#include <cstdio>
#include <cmath>
#include <cstdlib>
#include <cctype>
#include <thread>
#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"
#include "virtual_rtk.h"
#include "../mcu/mcu_if.h"
#include "../mcu/ahp.h"
#include "../utils/xconvert.h"

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

#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"};
const char CMD_VERSION[] = "log version\r\n";

static gpsStatus_t gpsStatus;
static char rtkModel[32] = {0};

static bool needSetPjk = false;
static int lostCnt;

// 组合PJK和TRA的消息内容
static struct {
    int year;
    int month;
    int day;
    int hour;
    int min;
    int sec;
    int millisec;
    int qf;
    double heading;
    double pitch;
    double roll;
    double x;
    double y;
} xy_temp;

static void CheckPjkParam(void);
static void CheckPjkParamTimeout(apptimer_var_t val);
static int WriteBluetooth(int id, const void *buf, int len);
static void GetModuleVersion(void);

static void VersionTimeout(apptimer_var_t val);
static void GpsDataTimeout(apptimer_var_t val);

// 蓝牙连接后
void ConfigRTKModuleLater(void)
{
    AppTimer_delete(GpsDataTimeout);
    AppTimer_delete(VersionTimeout);
    AppTimer_delete(CheckPjkParamTimeout);
    AppTimer_add(CheckPjkParamTimeout, D_SEC(1));
}

void FactorySettings(void)
{
    WriteRtkCommand(FACTORY, strlen(FACTORY));
}

void RebootModule(void)
{
    WriteRtkCommand(REBOOT, strlen(REBOOT));
}

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

void handlePJKParam(const struct nmea *s) {
    DEBUG("handlePJKParam");

    AppTimer_delete(CheckPjkParamTimeout);

    SetAYFactoryParam(5);
    needSetPjk = true;

    lostCnt = 0;
    AppTimer_delete(GpsDataTimeout);
    AppTimer_add(GpsDataTimeout, D_SEC(5));

    uint8_t buff[33];

    memcpy(buff, rtkModel, 32);
    buff[32] = 1;
    PlatformStatusChanged(RTK_STATUS_EVT, buff, 33);
}

void SetAYFactoryParam(int freq)
{
    WriteRtkCommand(UNLOGALL, strlen(UNLOGALL));
    WriteRtkCommand(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);
        WriteRtkCommand(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);
        WriteRtkCommand(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);

    WriteRtkCommand(buff, strlen(buff));

    DEBUG("%s", buff);
}

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

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

    if (length >= 100) {
        std::string cs(buff);

        if (cs.find("K708") != std::string::npos) {
            // 最初的标准基站模块
            DEBUG("K708 模块");
            strcpy(rtkModel, "K708");

            AppTimer_delete(VersionTimeout);
            CheckPjkParam();
        } else if (cs.find("K726") != std::string::npos) {
            // 移动站模块，也可以用做基站功能
            DEBUG("K726 模块");
            strcpy(rtkModel, "K726");

            AppTimer_delete(VersionTimeout);
            CheckPjkParam();
        }
    }
}

static bool bbs = false;

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

//    DEBUG("handleGPGGA num = %d", s->nmea_num);
    if (s->nmea_num >= 10) {

        if (!bbs) {
            bbs = true;
            RebootModule();
        }

        lostCnt = 0;
        AppTimer_delete(GpsDataTimeout);
        AppTimer_add(GpsDataTimeout, D_SEC(5));

        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);

    xy_temp.month = str2int(s->nmea_value[1].data, 2);
    xy_temp.day = str2int(s->nmea_value[1].data + 2, 2);
    xy_temp.year = str2int(s->nmea_value[1].data + 4, 2);

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

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

    if (xy_temp.hour == hh && xy_temp.min == mm && xy_temp.sec == ss && xy_temp.millisec == dss) {
        rtk_info_t rtk;
        rtk.qf = xy_temp.qf;
        rtk.utc_time = static_cast<int64_t>(TimeMakeComposite(2000 + xy_temp.year, xy_temp.month, xy_temp.day, xy_temp.hour, xy_temp.min, xy_temp.sec))*static_cast<int64_t>(1000) + static_cast<int64_t>(dss*10);
        rtk.x = xy_temp.x;
        rtk.y = xy_temp.y;
        rtk.heading = xy_temp.heading;
        rtk.pitch = xy_temp.pitch;
        rtk.roll = xy_temp.roll;
        PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&rtk, sizeof(rtk));
    }
    xy_temp.hour = hh;
    xy_temp.min = mm;
    xy_temp.sec = ss;
    xy_temp.millisec = 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(&xy_temp.heading, s->nmea_value[1].data, s->nmea_value[1].length);
    str2float(&xy_temp.pitch, s->nmea_value[2].data, s->nmea_value[2].length);
    str2float(&xy_temp.roll, s->nmea_value[3].data, s->nmea_value[3].length);

    if (xy_temp.hour == hh && xy_temp.min == mm && xy_temp.sec == ss && xy_temp.millisec == dss) {
        rtk_info_t rtk;
        rtk.qf = xy_temp.qf;
        rtk.utc_time = static_cast<int64_t>(TimeMakeComposite(2000 + xy_temp.year, xy_temp.month, xy_temp.day, xy_temp.hour, xy_temp.min, xy_temp.sec))*static_cast<int64_t>(1000) + static_cast<int64_t>(dss*10);
        rtk.x = xy_temp.x;
        rtk.y = xy_temp.y;
        rtk.heading = xy_temp.heading;
        rtk.pitch = xy_temp.pitch;
        rtk.roll = xy_temp.roll;
        PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&rtk, sizeof(rtk));
    }
    xy_temp.hour = hh;
    xy_temp.min = mm;
    xy_temp.sec = ss;
    xy_temp.millisec = dss;
}

static void CheckPjkParam(void)
{
    WriteRtkCommand(INQ_PJK_PARAM, strlen(INQ_PJK_PARAM));
    DEBUG("获取PJK参数...");
    AppTimer_delete(CheckPjkParamTimeout);
    AppTimer_add(CheckPjkParamTimeout, D_SEC(3));
}

static void CheckPjkParamTimeout(apptimer_var_t val) {
    DEBUG("获取PJK参数超时");

    uint8_t buff[33];
    memcpy(buff, rtkModel, 32);
    buff[32] = 0;
    PlatformStatusChanged(RTK_STATUS_EVT, buff, 33);

    GetModuleVersion();
}

static int WriteBluetooth(int id, const void *buf, int len)
{
//    SendToBluetooth((uint8_t *)buf, len);
//    SendMcuCommand(0x000B, (uint8_t *)buf, len);
    return len;
}

static void GetModuleVersion(void)
{
    AppTimer_delete(VersionTimeout);
    AppTimer_add(VersionTimeout, D_SEC(3));
    WriteRtkCommand(CMD_VERSION, strlen(CMD_VERSION));

    DEBUG("获取版本...");
}

static void VersionTimeout(apptimer_var_t val)
{
    DEBUG("版本获取超时");
    GetModuleVersion();

    uint8_t buff[33];
    memcpy(buff, rtkModel, 32);
    buff[32] = 0;
    PlatformStatusChanged(RTK_STATUS_EVT, buff, 33);
}

static void GpsDataTimeout(apptimer_var_t val)
{
    if (++lostCnt >= 3) {
        DEBUG("RTK模块收不到GPS数据");
        GetModuleVersion();
    } else {
        AppTimer_add(GpsDataTimeout, D_SEC(5));
    }
}