//
// Created by YY on 2019/10/21.
// Units note: distance - metre
//             speed - metre per second
//             angle - DEGREES
//

#include <cstdlib>
#include <cmath>
#include <semaphore.h>
#include <pthread.h>
#include <cstring>
#include <vector>
#include "driver_test.h"
#include "defs.h"
#include "Geometry.h"
#include "common/apptimer.h"
#include "jni_log.h"
#include "test_items/park_edge.h"
#include "native-lib.h"
#include "test_items/park_bottom.h"
#include "test_items/park_edge.h"
#include "test_items/error_list.h"
#include "test_items/turn_a90.h"
#include "test_items/driving_curve.h"
#include "test_items/stop_and_start.h"

using namespace std;

#define RTK_INVALID         0
#define RTK_SINGLE_POINT   1
#define RTK_DIFF            2
#define RTK_FIX            3
#define RTK_FLOAT            4

#define CAR_MODEL_POINT_NUM             32

enum {
    TEST_NONE,
    TEST_PARK_EDGE,
    TEST_PARK_BOTTOM,
    TEST_TUNE_90,
    TEST_S_CURVE,
    TEST_SLOPE,
    TEST_SIMPLE,
    TEST_END
};

static bool TestStart = false;
static int TestItem = TEST_NONE;
static int CarInArea = 0;
int errs = 0;

vector<int> ErrorList;

static Polygon theParkEdgeMap;         // 侧位停车地图
static Polygon theTurn90Map;
static Polygon theSSMap;

#define CAR_COORD_STORE_SIZE        11

struct car_coord_ {
    uint32_t uptime;
    double azimuth;
    PointF coord;
};

struct car_desc_ {
    double distance;            // 距离主天线的距离
    double angle;               // 从中轴线逆时针形成的角度
};

struct car_desc_ *CarDesc = NULL;

struct car_coord_ *CarCoord = NULL;
static double currSpeed = 0, currAzimuth = 0;
static PointF currCoord;
static int prevDirect = 0, currDirect = 0;

static int car_coord_num = 0, car_coord_in = 0;

#define MOV_AVG_SIZE                1
#define RTK_HISTORY_SIZE            1000
#define CAR_MODEL_CACHE_SIZE      10

struct rtk_info_ {
    int qf;
    time_t uptime;
    double azimuth;
    PointF coord;
};

static struct rtk_info_ *RTKHistory = NULL;
static int rtk_history_num = 0, rtk_history_in = 0;
static pthread_mutex_t add_rtk_history_mutex = PTHREAD_MUTEX_INITIALIZER;

static carModelDesc_t *carModelDescFile = NULL;

static car_model_cache_t carModelCache[CAR_MODEL_CACHE_SIZE];
static int carModelCacheIn, carModelCacheNum;

static void UpdateCarBodyCoord(double azimuth, PointF coord);
static void UpdateCarBodyCoord(double azimuth, PointF coord, car_model_cache_t *carModel, int &carModelIn, int &carModelNum);
static bool FrontTireEnterArea(const Polygon *car, const Polygon *map);

void DriverTestInit(void)
{
    carModelDescFile = (carModelDesc_t *)malloc(sizeof(carModelDesc_t));

    carModelDescFile->body_num = 6;
    carModelDescFile->body[0] = 0;
    carModelDescFile->body[1] = 1;
    carModelDescFile->body[2] = 2;
    carModelDescFile->body[3] = 3;
    carModelDescFile->body[4] = 4;
    carModelDescFile->body[5] = 5;

    carModelDescFile->front_left_tire[0] = 1;
    carModelDescFile->front_left_tire[1] = 1;
    carModelDescFile->front_right_tire[0] = 5;
    carModelDescFile->front_right_tire[1] = 5;
    carModelDescFile->rear_left_tire[0] = 2;
    carModelDescFile->rear_left_tire[1] = 2;
    carModelDescFile->rear_right_tire[0] = 4;
    carModelDescFile->rear_right_tire[1] = 4;

    memset(&carModelCache, 0, sizeof(carModelCache));
    carModelCacheIn = carModelCacheNum = 0;

    CarDesc = (struct car_desc_ *) malloc(sizeof(struct car_desc_) * 6);
    CarDesc[0].distance = 0.2465;
    CarDesc[0].angle = 0;

    CarDesc[1].distance = 0.2635;
    CarDesc[1].angle = 20.7;

    CarDesc[2].distance = 0.14;
    CarDesc[2].angle = 138.9;

    CarDesc[3].distance = 0.1055;
    CarDesc[3].angle = 180.0;

    CarDesc[4].distance = 0.14;
    CarDesc[4].angle = 221.1;

    CarDesc[5].distance = 0.2635;
    CarDesc[5].angle = 339.3;

    memset(&theParkEdgeMap, 0, sizeof(theParkEdgeMap));
    theParkEdgeMap.num = 8;
    theParkEdgeMap.point = (PointF *)malloc(theParkEdgeMap.num * sizeof(PointF));

    theParkEdgeMap.point[0].Y = 28.013;
    theParkEdgeMap.point[0].X = -11.9669;

    theParkEdgeMap.point[1].Y = 27.137;
    theParkEdgeMap.point[1].X = -11.5114;

    theParkEdgeMap.point[2].Y = 27.5039;
    theParkEdgeMap.point[2].X = -10.8069;

    theParkEdgeMap.point[3].Y = 26.4212;
    theParkEdgeMap.point[3].X = -10.2969;

    theParkEdgeMap.point[4].Y = 26.8894;
    theParkEdgeMap.point[4].X = -9.2102;

    theParkEdgeMap.point[5].Y = 28.0027;
    theParkEdgeMap.point[5].X = -9.6513;

    theParkEdgeMap.point[6].Y = 28.3797;
    theParkEdgeMap.point[6].X = -8.9758;

    theParkEdgeMap.point[7].Y = 29.3232;
    theParkEdgeMap.point[7].X = -9.5057;

    memset(&theTurn90Map, 0, sizeof(theTurn90Map));
    theTurn90Map.num = 6;
    theTurn90Map.point = (PointF *)malloc(theTurn90Map.num * sizeof(PointF));

    theTurn90Map.point[0].Y = 10;
    theTurn90Map.point[0].X = 10;

    theTurn90Map.point[1].Y = 12.5;
    theTurn90Map.point[1].X = 10;

    theTurn90Map.point[2].Y = 12.5;
    theTurn90Map.point[2].X = 12.5;

    theTurn90Map.point[3].Y = 15;
    theTurn90Map.point[3].X = 12.5;

    theTurn90Map.point[4].Y = 15;
    theTurn90Map.point[4].X = 15;

    theTurn90Map.point[5].Y = 10;
    theTurn90Map.point[5].X = 15;

    memset(&theSSMap, 0, sizeof(theSSMap));
    theSSMap.num = 9;
    theSSMap.point = (PointF *)malloc(theSSMap.num * sizeof(PointF));

    theSSMap.point[0].Y = 10;
    theSSMap.point[0].X = 10;

    theSSMap.point[8].Y = 10;
    theSSMap.point[8].X = 15;

    theSSMap.point[1].Y = 10;
    theSSMap.point[1].X = 11.5;

    theSSMap.point[4].Y = 10;
    theSSMap.point[4].X = 12.5;

    theSSMap.point[5].Y = 10;
    theSSMap.point[5].X = 13.5;

    theSSMap.point[2].Y = 12;
    theSSMap.point[2].X = 11.5;

    theSSMap.point[3].Y = 12;
    theSSMap.point[3].X = 12.5;

    theSSMap.point[6].Y = 12;
    theSSMap.point[6].X = 13.5;

    theSSMap.point[7].Y = 12;
    theSSMap.point[7].X = 15;

    pthread_mutex_init(&add_rtk_history_mutex, NULL);

    RTKHistory = (struct rtk_info_ *) malloc(RTK_HISTORY_SIZE * sizeof(struct rtk_info_));
    rtk_history_num = rtk_history_in = 0;

    CarCoord = (struct car_coord_ *) malloc(CAR_COORD_STORE_SIZE * sizeof(struct car_coord_));
    car_coord_num = car_coord_in = 0;

    TestStart = true;
    TestItem = TEST_NONE;

    ErrorList.clear();

    TextSpeak("开始测试");
}

void UpdateRTKInfo(struct rtk_info *s)
{
    struct tm test_tm;

    struct timeval tv;
    struct timezone tz;

    gettimeofday(&tv,&tz);

    memset(&test_tm, 0, sizeof(test_tm));

    test_tm.tm_year = 2000 + s->YY - 1900;
    test_tm.tm_mon = s->MM - 1;
    test_tm.tm_mday = s->DD;
    test_tm.tm_hour = s->hh;
    test_tm.tm_min = s->mm;
    test_tm.tm_sec = s->ss;

    pthread_mutex_lock(&add_rtk_history_mutex);
    /*if (s->qf == 3)*/ {
        RTKHistory[rtk_history_in].qf = RTK_FIX;//s->qf;
        RTKHistory[rtk_history_in].uptime = mktime(&test_tm) - tz.tz_minuteswest*60;
        RTKHistory[rtk_history_in].azimuth = s->heading;
        RTKHistory[rtk_history_in].coord.X = s->x;
        RTKHistory[rtk_history_in].coord.Y = s->y;

        rtk_history_in = (rtk_history_in + 1) % RTK_HISTORY_SIZE;
        if (rtk_history_num < RTK_HISTORY_SIZE)
            rtk_history_num++;
    }
    pthread_mutex_unlock(&add_rtk_history_mutex);

//    DEBUG("UpdateRTKInfo qf = %d tm %ld", s->qf, mktime(&test_tm) - tz.tz_minuteswest*60);
}

void UpdateCarCoord(void)
{
    double azimuth;
    PointF coord;

    // 计算最近的几个，滑动平均
    int cnt = 0, valid_cnt = 0;
    int s, sm;
    time_t prev_time = 0;
    time_t last_rtk_time = 0;

    pthread_mutex_lock(&add_rtk_history_mutex);
    s = rtk_history_in;
    sm = rtk_history_num;
    pthread_mutex_unlock(&add_rtk_history_mutex);

    DEBUG("UpdateCarCoord rtk_history_in %d rtk_history_num %d car_coord_num %d", s, sm, car_coord_num);

    // 如果出现QF不是固定解、GPS报文丢失的情况，就按上次的行驶状态做直线行驶
    for (; cnt < sm && valid_cnt < MOV_AVG_SIZE; cnt++) {
        if (s == 0) {
            s = RTK_HISTORY_SIZE - 1;
        } else {
            s--;
        }

        if (cnt == 0) {
            last_rtk_time = RTKHistory[s].uptime;
        }

        if (RTKHistory[s].uptime - last_rtk_time <= 10) {
            if (RTKHistory[s].qf == RTK_FIX) {
                if (valid_cnt != 0) {
                    azimuth += RTKHistory[s].azimuth;
                    coord.X += RTKHistory[s].coord.X;
                    coord.Y += RTKHistory[s].coord.Y;
                } else {
                    azimuth = RTKHistory[s].azimuth;
                    coord.X = RTKHistory[s].coord.X;
                    coord.Y = RTKHistory[s].coord.Y;
                }
                valid_cnt++;
            }
        } else {
            break;
        }
        prev_time = RTKHistory[s].uptime;
    }

    if (valid_cnt >= MOV_AVG_SIZE) {
        azimuth /= MOV_AVG_SIZE;
        coord.X /= MOV_AVG_SIZE;
        coord.Y /= MOV_AVG_SIZE;

        CarCoord[car_coord_in].uptime = AppTimer_GetTickCount();

        CarCoord[car_coord_in].azimuth = azimuth;
        CarCoord[car_coord_in].coord = coord;
        car_coord_in = (car_coord_in + 1) % CAR_COORD_STORE_SIZE;
        if (car_coord_num < CAR_COORD_STORE_SIZE)
            car_coord_num++;
    } else if (car_coord_num >= 3) {
        // 按上次运行轨迹推算一步
        int p1 = ((car_coord_in-1)+CAR_COORD_STORE_SIZE)%CAR_COORD_STORE_SIZE;
        uint32_t tm = AppTimer_GetTickCount() -  CarCoord[p1].uptime;
        double distance = currSpeed * tm / 1000;

        PointF xy;

        xy.X = CarCoord[p1].coord.X + distance * sin(toRadians(CarCoord[p1].azimuth)) * currDirect;
        xy.Y = CarCoord[p1].coord.Y + distance * sin(toRadians(CarCoord[p1].azimuth)) * currDirect;

        CarCoord[car_coord_in].uptime = AppTimer_GetTickCount();
        CarCoord[car_coord_in].azimuth =  CarCoord[p1].azimuth;
        CarCoord[car_coord_in].coord = xy;
        car_coord_in = (car_coord_in + 1) % CAR_COORD_STORE_SIZE;
        if (car_coord_num < CAR_COORD_STORE_SIZE)
            car_coord_num++;
    } else {
        return;
    }

    // 计算车辆轮廓点、速度、前进后退
    if (car_coord_num >= 3) {
        int p1 = ((car_coord_in-1)+CAR_COORD_STORE_SIZE)%CAR_COORD_STORE_SIZE;
        int p2 = ((car_coord_in-2)+CAR_COORD_STORE_SIZE)%CAR_COORD_STORE_SIZE;
        int p3 = ((car_coord_in-3)+CAR_COORD_STORE_SIZE)%CAR_COORD_STORE_SIZE;

        double speed1 = sqrt(pow(CarCoord[p1].coord.X - CarCoord[p2].coord.X, 2) + pow(CarCoord[p1].coord.Y - CarCoord[p2].coord.Y, 2)) * 1000 /
                (double)(CarCoord[p1].uptime - CarCoord[p2].uptime);
        double speed2 = sqrt(pow(CarCoord[p2].coord.X - CarCoord[p3].coord.X, 2) + pow(CarCoord[p2].coord.Y - CarCoord[p3].coord.Y, 2)) * 1000 /
                        (double)(CarCoord[p2].uptime - CarCoord[p3].uptime);

        currSpeed = speed1;
        currAzimuth = CarCoord[p1].azimuth;
        currCoord = CarCoord[p1].coord;

        DEBUG("方向 spd1 %f spd2 %f", speed1, speed2);

        if (speed1 < 0.05 && speed2 < 0.05) {
            // 停车
            currDirect = 0;
//            TextSpeak("停");
        } else if (speed1 < 0.05) {
             currDirect = prevDirect;
        } else {
            // 判断前进还是后退
            double deg = 0.0;

            if (fabs(CarCoord[p1].coord.Y-CarCoord[p2].coord.Y) <= GLB_EPSILON) {
                if (CarCoord[p1].coord.X > CarCoord[p2].coord.X) {
                    deg = 90;
                } else {
                    deg = 270;
                }

                DEBUG("方向 deg %f p1x %f p2x %f", deg, CarCoord[p1].coord.X, CarCoord[p2].coord.X);
            } else if (fabs(CarCoord[p1].coord.X - CarCoord[p2].coord.X) <= GLB_EPSILON) {
                if (CarCoord[p1].coord.Y > CarCoord[p2].coord.Y) {
                    deg = 0;
                } else {
                    deg = 180;
                }
            } else {
                deg = atan(fabs(CarCoord[p1].coord.X - CarCoord[p2].coord.X) /
                           fabs(CarCoord[p1].coord.Y - CarCoord[p2].coord.Y));

                deg = toDegree(deg);

                if (CarCoord[p1].coord.X > CarCoord[p2].coord.X &&
                    CarCoord[p1].coord.Y > CarCoord[p2].coord.Y) {

                } else if (CarCoord[p1].coord.X < CarCoord[p2].coord.X &&
                           CarCoord[p1].coord.Y > CarCoord[p2].coord.Y) {
                    deg = 360 - deg;
                } else if (CarCoord[p1].coord.X < CarCoord[p2].coord.X &&
                           CarCoord[p1].coord.Y < CarCoord[p2].coord.Y) {
                    deg = 180 + deg;
                } else if (CarCoord[p1].coord.X > CarCoord[p2].coord.X &&
                           CarCoord[p1].coord.Y < CarCoord[p2].coord.Y) {
                    deg = 180 - deg;
                }

                DEBUG("方向 deg %f p1x %f p2x %f p1y %f p2y %f", deg, CarCoord[p1].coord.X,
                      CarCoord[p2].coord.X, CarCoord[p1].coord.Y, CarCoord[p2].coord.Y);
            }

            deg = fabs(CarCoord[p1].azimuth - deg);
            if (deg > 180) {
                deg = 360 - deg;
            }
            if (deg < 90) {
                // 前进
                currDirect = 1;
//                    TextSpeak("进");
            } else {
                // 后退
                currDirect = -1;
//                    TextSpeak("退");
            }

            prevDirect = currDirect;
        }

        DEBUG("speed = %f, azimuth = %f coord.X = %f coord.Y = %f dir = %d", speed1, CarCoord[p1].azimuth,
              CarCoord[p1].coord.X, CarCoord[p1].coord.Y, currDirect);

//        UpdateCarBodyCoord(CarCoord[p1].azimuth, CarCoord[p1].coord);
        UpdateCarBodyCoord(CarCoord[p1].azimuth, CarCoord[p1].coord, carModelCache, carModelCacheIn, carModelCacheNum);
//        for (int i = 0; i < theCarModel.num; ++i) {
//            DEBUG("n = %d X = %f Y = %f", i, theCarModel.point[i].X, theCarModel.point[i].Y);
//        }


        int c1 = ((carModelCacheIn-1)+CAR_MODEL_CACHE_SIZE)%CAR_MODEL_CACHE_SIZE;
        Polygon py;

        py.num = carModelCache[c1].point_num;
        py.point = carModelCache[c1].points;

        DrawScreen(&theSSMap, &py);

        if (!TestStart) return;

        if (CarInArea == 0) {
//            if (FrontTireEnterArea(&py, &theParkEdgeMap)) {
//                CarInArea = TEST_PARK_BOTTOM;
//                TestItem = TEST_PARK_BOTTOM;
//                StartParkBottom();
//            }

            CarInArea = TEST_SLOPE;
            TestItem = TEST_SLOPE;
            StartSAS();
        }

        switch (TestItem) {
            case TEST_NONE: {
                break;
            }
            case TEST_PARK_BOTTOM: {
                errs = TestParkBottom(ErrorList, &theParkEdgeMap, GetCarModelCache(0), currSpeed, currDirect);

                if (errs != 0) {
                    StopParkBottom();
                    TestItem = TEST_NONE;
                }
                break;
            }
            case TEST_PARK_EDGE: {
                errs = TestParkEdge(ErrorList, &theParkEdgeMap, GetCarModelCache(0), currSpeed, currDirect);

                if (errs != 0) {
                    StopParkEdge();
                    TestItem = TEST_NONE;
                }
                break;
            }

            case TEST_TUNE_90: {
                errs = TestTurnA90(ErrorList, &theTurn90Map, GetCarModelCache(0), currSpeed, currDirect, currAzimuth);

                if (errs != 0) {
                    StopTurnA90();
                    TestItem = TEST_NONE;
                }

                break;
            }

            case TEST_SLOPE: {
                errs = TestSAS(ErrorList, &theSSMap, GetCarModelCache(0), currSpeed, currDirect);

                if (errs != 0) {
                    StopSAS();
                    TestItem = TEST_NONE;
                }

                break;
            }
            default:
                break;
        }

        if (ErrorList.size() > 0) {
            vector<int>::iterator it = ErrorList.end();
            it--;
            error_list_t list = GetErrorList(*it);

            int scr = 0;

            for (vector<int>::iterator it1 = ErrorList.begin(); it1 != ErrorList.end(); ++it1) {
                error_list_t list = GetErrorList(*it1);

                scr += list.dec_score;
            }

            char buff[256];

            sprintf(buff, "%s, 总计扣分 %d", list.text_desc, scr);

            TextOsd(1, buff);
        }
    }
}

car_model_cache_t *GetCarModelCache(int node)
{
    if (node < carModelCacheNum) {
        node = ((carModelCacheIn-node-1)+CAR_MODEL_CACHE_SIZE)%CAR_MODEL_CACHE_SIZE;
        return &carModelCache[node];
    }
    return NULL;
}

/*******************************************************************
 * @brief 由主天线坐标计算车身点坐标
 * @param azimuth
 * @param coord
 */
static void UpdateCarBodyCoord(double azimuth, PointF coord, car_model_cache_t *carModel, int &in, int &num)
{
    carModel[in].uptime = AppTimer_GetTickCount();
    carModel[in].desc = carModelDescFile;
    carModel[in].point_num = 6;

    if (carModel[in].points == NULL) {
        carModel[in].points = (PointF *) malloc(sizeof(PointF) * carModel[in].point_num);
    }

    for (int i = 0; i < 6; ++i) {
        double tx = coord.X + CarDesc[i].distance*sin(toRadians(azimuth));
        double ty = coord.Y + CarDesc[i].distance*cos(toRadians(azimuth));

        carModel[in].points[i].X = (tx-coord.X)*cos(toRadians(CarDesc[i].angle)) -
                                 (ty-coord.Y)*sin(toRadians(CarDesc[i].angle)) + coord.X;
        carModel[in].points[i].Y = (tx-coord.X)*sin(toRadians(CarDesc[i].angle)) +
                                 (ty-coord.Y)*cos(toRadians(CarDesc[i].angle)) + coord.Y;
    }

    in = (in + 1) % CAR_MODEL_CACHE_SIZE;
    if (num < CAR_MODEL_CACHE_SIZE) {
        num++;
    }
}

static void UpdateCarBodyCoord(double azimuth, PointF coord)
{
//    static double az = 0;
//
//    az += 2.0;
//
//    for (int i = 0; i < theCarModel.num; ++i) {
//        double tx = coord.X + CarDesc[i].distance*sin(toRadians(azimuth));
//        double ty = coord.Y + CarDesc[i].distance*cos(toRadians(azimuth));
//
//        theCarModel.point[i].X = (tx-coord.X)*cos(toRadians(CarDesc[i].angle)) -
//                                 (ty-coord.Y)*sin(toRadians(CarDesc[i].angle)) + coord.X;
//        theCarModel.point[i].Y = (tx-coord.X)*sin(toRadians(CarDesc[i].angle)) +
//                                 (ty-coord.Y)*cos(toRadians(CarDesc[i].angle)) + coord.Y;
//    }
//    UpdateParkTest(&theCarModel, &theTireModel, speed);
//    UpdatePark2Test(&theCarModel, &theTireModel, speed);
//    UpdateTuneAngle90Test(&theCarModel, 0);
    //UpdateSlopeTest(&theCarModel, speed, direct);
}

static bool FrontTireEnterArea(const Polygon *car, const Polygon *map)
{
    if (IntersectionOf(car->point[0], map) == GM_Containment) {
        return true;
    }
    return false;
}