#include "trajectoryplanner.h"

bool isAngleBetweenTwoAngles(float angle, float start_angle, float end_angle)
{
    bool angle_match = false;

    // ?? ???????
    if ((angle>=start_angle)&&(angle<=end_angle)) {
        angle_match = true;
    } else {
        angle_match = false;
    }

    return angle_match;
}

float TrajectoryElement::getDistanceToPoint(const RobotPosition &element_pos, const QPointF &point)
{
    float result = 10000000.0f;
    if (type==ElementType_Line) {
        // ?????????? ?? ???????
        QPointF end_pos = getEndpointPosition(element_pos);
        QVector2D v1(point-element_pos);
        QVector2D v2(end_pos-element_pos);
        float product = QVector2D::dotProduct(v1,v2);

        QPointF p = QPointF(element_pos) + (end_pos-element_pos)*(product/v2.lengthSquared());

        float line_sign = sign(float(v1.x()*v2.y()-v1.y()*v2.x()));

        if ((product>=0.0)&&((product/v2.lengthSquared())<=1.0)) {
            // ????? ????? ??????
            result = line_sign*QVector2D(p-point).length();
        } else {
            // ????? ???????
            if (product<0) // ?????
                result = line_sign*QVector2D(element_pos-point).length();
            else //  ??????
                result = line_sign*QVector2D(end_pos-point).length();
        }
    } else if (type==ElementType_Arc) {
        // ?????????? ?? ????
        // ?????????? ????? ??????????
        QPointF rot = getArcCenter(element_pos);
        // ??????? ????? ? ?????? ????????? ? rot
        QPointF pos = (point-rot);
        QPointF start_pos = (element_pos-rot);
        // ?????????? ????????? ???? ????
        float start_angle_offset = Angle(atan2(start_pos.y(), start_pos.x()));
        float start_angle = Angle(atan2(start_pos.y(), start_pos.x())-start_angle_offset).to2PIFormat();
        start_angle = 0;
        // ???????? ????
        float angle_wise = sign(angle)*sign(R);
        float end_angle = Angle(fabs(angle)).to2PIFormat();


        // ?????????? ???? ????? ?????? ? ???????
        float point_angle1 = 0;
        float point_angle2 = 0;
        if (angle_wise>0) {
            point_angle1 = PIx2-Angle(atan2(pos.y(), pos.x()) - start_angle_offset).to2PIFormat();  // ???? ?? ?????? ? ?????
            point_angle2 = PIx2-Angle(atan2(pos.y(), pos.x())+PI - start_angle_offset).to2PIFormat();   // ??????????????? ????
        } else {
            point_angle1 = Angle(atan2(pos.y(), pos.x()) - start_angle_offset).to2PIFormat();  // ???? ?? ?????? ? ?????
            point_angle2 = Angle(atan2(pos.y(), pos.x())+PI - start_angle_offset).to2PIFormat();   // ??????????????? ????
        }
        //qDebug()<<point_angle1;


        // ??????? ???????? ?? ??? ???? ? ????????

        bool angle_match_short = false;
        bool angle_match_long = false;
        if (fabs(angle)<PIx2) {
            //
            angle_match_long = isAngleBetweenTwoAngles(point_angle1, start_angle, end_angle);
            angle_match_short = isAngleBetweenTwoAngles(point_angle2, start_angle, end_angle);
        } else { // ???? ?????? ??? PI*2 ?? 100% ???????????
            angle_match_short = true;
        }

        //qDebug()<<angle_match_long;

        // ?????????? ??????????
        if (angle_match_short) {
            // ?????????? ?? ?????? - ??????
            result = R+angle_wise*QVector2D(pos).length();
        } else if (angle_match_long) { // ?????????? ?? ?????? + ??????
            result = R-angle_wise*QVector2D(pos).length();
        } else {
            // ????????? ?????????? ?? ????? ?? ???? ?????
            // ??? ?? ????????????
        }
    }
    return result;
}

RobotPosition TrajectoryElement::getEndpointPosition(const RobotPosition &start_pos)
{
    RobotPosition pos;
    if (type==ElementType_Line) {
        pos.a = start_pos.a;
        pos.x = start_pos.x + line_length*cos(pos.a);
        pos.y = start_pos.y + line_length*sin(pos.a);

    } else if (type==ElementType_Arc) {
        pos.a = start_pos.a - angle*sign(R);
        // ????? ?????? ??????? ???????
        QPointF rot = getArcCenter(start_pos);
        //qDebug()<<rot;

        float an = -angle*sign(R);
        //qDebug()<<an;
        // ???????
        float tx = rot.x()+(start_pos.x-rot.x())*cos(an)-(start_pos.y-rot.y())*sin(an);
        float ty = rot.y()+(start_pos.x-rot.x())*sin(an)+(start_pos.y-rot.y())*cos(an);

        pos.x = tx;
        pos.y = ty;
    } else if (type==ElementType_Rotation) {
        pos.a = start_pos.a+angle;
        pos.x = start_pos.x;
        pos.y = start_pos.y;
    }
    pos.speed = endpoint_speed;
    return pos;
}

RobotPosition TrajectoryElement::interpolatePosition(const RobotPosition &start_pos, float value)
{
    RobotPosition pos;
    if (type==ElementType_Line) {
        pos.a = start_pos.a;
        pos.x = start_pos.x + line_length*cos(pos.a)*value;
        pos.y = start_pos.y + line_length*sin(pos.a)*value;

    } else if (type==ElementType_Arc) {
        float an = -angle*sign(R)*value;
        pos.a = start_pos.a - an;
        // ????? ?????? ??????? ???????
        QPointF rot = getArcCenter(start_pos);
        //qDebug()<<rot;

        //qDebug()<<an;
        // ???????
        float tx = rot.x()+(start_pos.x-rot.x())*cos(an)-(start_pos.y-rot.y())*sin(an);
        float ty = rot.y()+(start_pos.x-rot.x())*sin(an)+(start_pos.y-rot.y())*cos(an);

        pos.x = tx;
        pos.y = ty;
    } else if (type==ElementType_Rotation) {
        pos.a = start_pos.a+angle*value;
        pos.x = start_pos.x;
        pos.y = start_pos.y;
    }
    pos.speed = startpoint_speed*(1.0-value)+endpoint_speed*value;
    return pos;
}

QPointF TrajectoryElement::getArcCenter(const RobotPosition &start_pos)
{
    return QPointF(start_pos.x-R*cos(start_pos.a+PId2), start_pos.y-R*sin(start_pos.a+PId2));
}

float ITrajectoryPlanner::getTrajectoryLength(const Trajectory &trajectory)
{
    float res = 0;
    for (int i=0; i<trajectory.size(); i++) {
        if (trajectory.elements[i].type==ElementType_Arc) {
            res += sqr((trajectory.elements[i].R * trajectory.elements[i].angle * 10.0f));  // ???? ????? ????????????????, ??? ??????, ??????? ??? ??????
            //res += abs(trajectory[i].R * trajectory[i].angle );
        } else
            if (trajectory.elements[i].type==ElementType_Line) {
                res += fabs(trajectory.elements[i].line_length);
            }
    }

    return res;
}

float TrajectoryElement::getElementLength()
{
    if (type==ElementType_Line)
        return fabs(line_length);
    if (type==ElementType_Arc)
        return fabs(R*angle);
    return 0.0f;
}

float Trajectory::getDistanceToPoint(const RobotPosition &start_pos, const QPointF &point)
{
    RobotPosition element_position = start_pos;
    int size = elements.size();
    float min_size = 10000000.0;

    for (int i=0; i<size; i++) {
        float dist = elements[i].getDistanceToPoint(element_position, point);
        if (fabs(dist)<fabs(min_size)) {
            min_size = dist;
        }
        element_position = elements[i].getEndpointPosition(element_position);
        //qDebug()<<QString("%1  %2;%3").arg(i).arg(element_position.x).arg(element_position.y);
    }
    return min_size;
}


void Trajectory::appendTrajectory(const Trajectory &trajectory)
{
    for (int i=0; i<trajectory.size(); i++) {
        elements.push_back(trajectory.elements[i]);
    }
}