#include <adc_planner/lane_change_planner.h>
#include <adc_tools/transfer.h>
#include <adc_tools/parameter_status.h>
#include <common/constants.h>

#include <math.h>

iv::decition::LaneChangePlanner::LaneChangePlanner(){
    this->planner_id = 0;
    this->planner_name = "LaneChange";
}

iv::decition::LaneChangePlanner::~LaneChangePlanner(){
}

/**
 * @brief iv::decition::LaneChangePlanner::getPath
 * 采用的车道变换的局部路径生成方法。计算原始地图路径偏移offset距离之后的路径。
 *
 * @param now_gps_ins          实时gps信息
 * @param gpsMapLine           地图数据点
 * @param PathPoint            地图路线中距离车辆位置最近的一个点的序号
 * @param offset               在车辆避障中，地图源路线所在车道 与 即将去往的车道 之间的距离差值。可以为负值。
 * @param lidarGridPtr         激光雷达信息网格
 * @return                     返回一条车辆坐标系下的路径
 */
std::vector<iv::Point2D> iv::decition::LaneChangePlanner::getPath(GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint, double offset, iv::LidarGridPtr lidarGridPtr){
    std::vector<iv::Point2D> trace;
    trace = this->getGpsTrace(now_gps_ins,gpsMapLine,PathPoint);
    if(offset!=0){
        trace = this->getGpsTraceOffset(trace,offset);
    }
    return trace;
}

bool iv::decition::LaneChangePlanner::checkAvoidEnable(const std::vector<double>& obsDisVector, int roadAim){
    int roadNow = ServiceParameterStatus.now_road_num;
    if ((obsDisVector[roadAim]>0 && obsDisVector[roadAim]<obsDisVector[roadNow]+ServiceCarStatus.msysparam.vehLenth)||
            (obsDisVector[roadAim]>0 && obsDisVector[roadAim]<15))
    {
        return false;
    }

    if (roadAim-roadNow>1)
    {
        for (int i = roadNow+1; i < roadAim; i++)
        {
            if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
                return false;
            }
        }

    }

    else if (roadNow-roadAim>1)
    {
        for (int i = roadNow-1; i >roadAim; i--)
        {
            if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
                return false;
            }
        }
    }
    return true;
}

std::vector<iv::Point2D> iv::decition::LaneChangePlanner::getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData>& gpsMapLine, int PathPoint){
    std::vector<iv::Point2D> trace;

    if (gpsMapLine.size() > 400 && PathPoint >= 0) {
        for (int i = PathPoint; i < PathPoint + 600; i++)
        {
            int index = i % gpsMapLine.size();
            Point2D pt = Coordinate_Transfer((*gpsMapLine[index]).gps_x, (*gpsMapLine[index]).gps_y, now_gps_ins);
//            pt.x += offset_real * 0.032;
            pt.v1 = (*gpsMapLine[index]).speed_mode;
            pt.v2 = (*gpsMapLine[index]).mode2;
            pt.roadMode=(*gpsMapLine[index]).roadMode;

//            if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
//            {
//                readyZhucheMode = true;
//                DecideGps00::zhuchePointNum = index;
//            }



//            if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
//            {
//                readyZhucheMode = true;
//                DecideGps00::zhuchePointNum = index;
//            }

//            //csvv7
//            if (gpsMapLine[index]->speed_mode==22 || gpsMapLine[index]->speed_mode==23)
//            {
//                readyParkMode = true;
//                DecideGps00::finishPointNum = index;
//            }


            switch (pt.v1)
            {
            case 0:
                pt.speed = 36;
                break;
            case 1:
                pt.speed = 25;
                break;
            case 2:
                pt.speed =25;
                break;
            case 3:
                pt.speed = 20;
                break;
            case 4:
                pt.speed =18;
                break;
            case 5:
                pt.speed = 18;
                break;
            case 7:
                pt.speed = 10;
                break;
            case 22:
                pt.speed = 5;
                break;
            case 23:
                pt.speed = 5;
                break;
            default:
                break;
            }
            trace.push_back(pt);
        }
    }
    return trace;
}

std::vector<iv::Point2D> iv::decition::LaneChangePlanner::getGpsTraceOffset(const std::vector<Point2D>& gpsTrace, double offset){
    if (offset==0)
    {
        return gpsTrace;
    }

    std::vector<iv::Point2D> trace;
    for (int j = 0; j < gpsTrace.size(); j++)
    {
        double sumx1 = 0, sumy1 = 0, count1 = 0;
        double sumx2 = 0, sumy2 = 0, count2 = 0;
        for (int k = std::max(0, j - 4); k <= j; k++)
        {
            count1 = count1 + 1;
            sumx1 += gpsTrace[k].x;
            sumy1 += gpsTrace[k].y;
        }
        for (int k = j; k <= std::min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
        {
            count2 = count2 + 1;
            sumx2 += gpsTrace[k].x;
            sumy2 += gpsTrace[k].y;
        }
        sumx1 /= count1; sumy1 /= count1;
        sumx2 /= count2; sumy2 /= count2;

        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);

        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);

        double avoidLenth = abs(offset);
        if (offset<0)
        {
            Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + M_PI / 2),
                           carFronty + avoidLenth  * sin(anglevalue + M_PI / 2));
            ptLeft.speed = gpsTrace[j].speed;
            ptLeft.v1 = gpsTrace[j].v1;
            ptLeft.v2 = gpsTrace[j].v2;
            ptLeft.roadMode = gpsTrace[j].roadMode;
            trace.push_back(ptLeft);
        }
        else
        {
            Point2D ptRight(carFrontx + avoidLenth  * cos(anglevalue - M_PI / 2),
                            carFronty + avoidLenth  * sin(anglevalue - M_PI / 2));
            ptRight.speed = gpsTrace[j].speed;
            ptRight.v1 = gpsTrace[j].v1;
            ptRight.v2 = gpsTrace[j].v2;
            ptRight.roadMode = gpsTrace[j].roadMode;
            trace.push_back(ptRight);
        }
    }
    return trace;
}