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

#include <tuple>
#include "train.h"
#include "../driver_test.h"
#include "../jni_log.h"

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

// 得到点后，相应场地的坐标变换
// coordinate transformation
void train(prime_t &prime)
{
    park_button_map_t az;

    az.points.resize(std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points.size());

    // 以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;

    for (int i = 0; i < std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points.size(); ++i) {
        az.points[i] = {.X = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[i].X - dx,
                        .Y = std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[i].Y - dy};
    }

    // 以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;
    }

    for (int i = 0; i < az.points.size(); ++i) {
        if (i != 2) {
            az.points[i] = rotatePoint(az.points[i], az.points[2], angle);
        }
    }

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

    car = rotatePoint(car, az.points[2], angle);

    DEBUG("坐标变换后的 %f,%f", car.X, car.Y);
}