//
// Created by YY on 2023/4/20.
//

#include "teach.h"
#include "../jni_log.h"
#include "../driver_test.h"
#include "../native-lib.h"
#include <tuple>
#include <vector>
#include <algorithm>

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

typedef struct {
    int id;
    std::string tts;
    int stage;
    int refer;      // 车身某点，某某事件
    PointF point;   // 采集点经过坐标变换后得到
} point_tip_t;

typedef struct {
    int id;
    std::string tts;
    int stage;
    int refer;      // 车身某点，某某事件
    double distance;
    Line line;   // 采集点经过坐标变换后得到
} line_tip_t;

typedef struct {
    int item;
    std::vector<point_tip_t> tips;
} teach_tips_t;

typedef struct {

};

teach_tips_t all;

void testAll(void)
{
    all.item = MAP_TYPE_PARK_BUTTOM;

    point_tip_t p1 = {
            .id = 1,
            .tts = "提示一",
            .stage = 2,
            .refer = 1,
            .point = {.X = 4.396647, .Y = 4.967977}
    };

    point_tip_t p2 = {
            .id = 2,
            .tts = "提示二",
            .stage = 2,
            .refer = 1,
            .point = {.X = 1.352043, .Y = 3.434146}
    };

    point_tip_t p3 = {
            .id = 3,
            .tts = "提示三",
            .stage = 2,
            .refer = 1,
            .point = {.X = 1.042866, .Y = 0.124397}
    };

    point_tip_t p4 = {
            .id = 4,
            .tts = "提示四",
            .stage = 2,
            .refer = 1,
            .point = {.X = 1.037644, .Y = -4.675115}
    };

    all.tips.push_back(p1);
    all.tips.push_back(p2);
    all.tips.push_back(p3);
    all.tips.push_back(p4);
}

class Foobar {
public:
    int id;
    double distance;

    Foobar(int id, double distance): id(id), distance(distance) {}
    ~Foobar() = default;

    bool operator<(const Foobar &bar)
    {
        if (this->distance < bar.distance) {
            return true;
        }
        return false;
    }

    bool static decrease(const Foobar &foo1, const Foobar &foo2)
    {
        if (foo1.distance > foo2.distance)
        {
            return false;
        }
        return true;
    }

};

std::vector<Foobar> status;

// 当前项目阶段发生变化时，产生的点位提示
void LoadStageTips(prime_t &prime)
{
    DEBUG("LoadStageTips %d", prime.examing_area.stage);

    std::vector<Foobar>().swap(status);

    // 以2号点(左侧车库入口角)为原点
    double dx = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2].X;
    double dy = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2].Y;

    // 以2---->3线为-Y轴，旋转
    double angle = YawOf(std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[3]);

    if (angle > 180) {
        angle = angle - 180;
    } else {
        angle = angle + 180;
    }

    // 记录车辆位置经过坐标变换后的值
    PointF car = {.X = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]].X - dx,
            .Y = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]].Y - dy};

    car = rotatePoint(car, {.X = 0,.Y = 0}, angle);

    // 需按照距离远近递增排序
    for (int i = 0; i < all.tips.size(); ++i) {
        if (all.tips[i].stage == prime.examing_area.stage) {
            DEBUG("Add %d: %f", i, DistanceOf(car, all.tips[i].point));
            status.push_back(Foobar(i, DistanceOf(car, all.tips[i].point)));
        }
    }
    if (status.size() > 0) {
        std::sort(status.begin(), status.end(), Foobar::decrease);

        for (auto st: status) {
            DEBUG("%d: %f", st.id, st.distance);
        }
    }
}

void teach(prime_t &prime)
{
    if(status.size() == 0)
        return;

    PointF car;
    PointF &teach_point = all.tips[status.front().id].point;

    switch (prime.examing_area.type) {
        case MAP_TYPE_PARK_BUTTOM: {
            // 以2号点(左侧车库入口角)为原点
            double dx = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2].X;
            double dy = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2].Y;

            // 以2---->3线为-Y轴，旋转
            double angle = YawOf(std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[2], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[3]);

            if (angle > 180) {
                angle = angle - 180;
            } else {
                angle = angle + 180;
            }

            // 记录车辆位置经过坐标变换后的值
            car = {.X = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]].X - dx,
                    .Y = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]].Y - dy};

            car = rotatePoint(car, {.X = 0,.Y = 0}, angle);

            break;
        }
        case MAP_TYPE_PARK_EDGE:
            break;
        case MAP_TYPE_RIGHT_CORNER:
            break;
        case MAP_TYPE_UPHILL:
            break;
        case MAP_TYPE_CURVE:
            break;
        default:break;
    }

    double dis = DistanceOf(car, teach_point);

    DEBUG("XY(%f,%f, dis = %f)", car.X, car.Y, dis);

    if (dis < status.front().distance) {
        status.front().distance = dis;
    } else if (dis > status.front().distance && dis - status.front().distance > 0.05) {
        // 提示
        PlayTTS(all.tips[status.front().id].tts, nullptr);
        status.erase(status.begin());
    }
}