#include "agent.h"
#include <iostream>
#include <cmath>

Agent::Agent(AgentState init_state, double init_raduis, double init_blind_angle, int init_number):
    state(init_state), raduis(init_raduis), blind_angle(init_blind_angle), number(init_number)
{
    velocity_max.linear_velocity = 0.1;
    velocity_max.angular_velocity = 1;
    velocity.angular_velocity = 0.0;
    velocity.linear_velocity = 0.0;
    //用自身坐标初始化目标点
    target.x = state.x;
    target.y = state.y;
}

/**************************Parameter************************************/
void Agent::UpdateState(double v_linear, double v_angular)
{
    state.x = state.x + v_linear*cos(state.head);
    state.y = state.y + v_linear*sin(state.head);
    state.head = state.head + v_angular;
    //0-2pi yaw angle
    if(state.head > 2*M_PI)
        state.head = state.head - int(state.head / (2*M_PI)) * 2 * M_PI;
    else if(state.head < 0)
        state.head = state.head + int(-state.head / (2*M_PI) + 1) * 2 * M_PI;
}

void Agent::SetState(AgentState state)
{
    this->state = state;
}

AgentState Agent::GetState()
{
    return this->state;
}

Point Agent::GetPosition()
{
    Point p = {state.x, state.y};
    return p;
}

void Agent::AppendNeighbors(AgentState neighbor)
{
    this->neigbors.push_back(neighbor);
}
std::vector<AgentState> Agent::GetNeighbors()
{
    return neigbors;
}
void Agent::ClearNeighbors()
{
    this->neigbors.clear();
}
double Agent::GetRaduis()
{
    return this->raduis;
}

double Agent::GetHead()
{
    return state.head;
}
double Agent::GetBlindAngle()
{
    return blind_angle;
}

Eigen::Vector2d Agent::GetHeadVector()
{
    return Eigen::Vector2d(cos(state.head),sin(state.head));
}

//判断点是否在前面
bool Agent::isInHead(Point p)
{
    bool flag = false;
    Eigen::Vector2d s2p = {p.x - state.x, p.y - state.y};
    Eigen::Vector2d h = GetHeadVector();
    double alpha = acos(s2p.dot(h) / s2p.norm());
    if(alpha <= M_PI/2) return true;
    else return false;

}


/******************************Algrithm***************************************/
/*calculate by neighbors*/
AgentConstraint Agent::CalculateConstraint()
{
    constraint.linear = velocity_max.linear_velocity;
    constraint.angularL = velocity_max.angular_velocity;
    constraint.angularR = velocity_max.angular_velocity;

    double beta_temp, linear_temp, angular_temp, distance_to_boundary;
    for(auto n:neigbors)
    {
        Eigen::Vector2d n2self(state.x - n.x, state.y - n.y);
        Eigen::Vector2d nighbor_head =GetVecFromHead(n.head);
        double dist;
        /*not in other's blind area, ensure undirected edge*/
        if(acos(nighbor_head.dot(n2self)/(nighbor_head.norm()*n2self.norm())) > (M_PI - blind_angle/2))
            continue;

        /*1.linear velocity limit by circular bundary*/
        dist = n2self.norm();
        // 这里假设neighbor与自己的半径相同, 后面写算法需要注意
        constraint.linear = std::min(constraint.linear, (raduis-dist)/2);

        /*2.linear constraint from other agent blind area of neighbor*/
        distance_to_boundary = GetDist2Bound(n, this->state, this->blind_angle);
        constraint.linear = std::min(constraint.linear, distance_to_boundary/2);

        /*3.linear&angular constraint from other agent position*/
        distance_to_boundary = GetDist2Bound(this->state, n, this->blind_angle);
        linear_temp = std::min(velocity_max.linear_velocity, distance_to_boundary/2); //assume neighbor linear constraints
        constraint.linear = std::min(constraint.linear, linear_temp);  //update self linear
        beta_temp = acos((-GetHeadVector()).dot(-n2self)/(GetHeadVector().norm()*dist))-blind_angle/2;
        angular_temp = beta_temp - asin((linear_temp+constraint.linear)/dist); //拿估计邻居的线速度与自己真实的线速度约束计算
        if(GetHeadVector().cross(-n2self) >= 0)
            constraint.angularL = std::min(constraint.angularL, angular_temp);
        else
            constraint.angularR = std::min(constraint.angularR, angular_temp);
    }

    return constraint;
}

/*calculate the average point by neighbors (cotain self)*/
Point Agent::GetAveragePoint()
{
    Point average = {0, 0};
    for(auto n:neigbors)
    {
        average.x += n.x;
        average.y += n.y;
    }

    average.x = average.x / neigbors.size();
    average.y = average.y / neigbors.size();
    return average;
}

//以平均点为目标点
void Agent::MoveAverageStrategy()
{
    double v_linear, v_angular;
    double dist_linear, dist_angular;
    target = GetAveragePoint();
    Eigen::Vector2d p2a = (target - GetPosition()).toVector();
    dist_linear = p2a.norm();
    dist_angular = acos(p2a.dot(GetHeadVector())/dist_linear); //0-2pi
    CalculateConstraint();
    if(isInHead(target)) //目标点在前面
    {
        v_linear = std::min(constraint.linear, dist_linear/2);
        if(isRight(state, target)) v_angular = -std::min(dist_angular, constraint.angularL);
        else v_angular = std::min(dist_angular, constraint.angularR);
    }
    else                                       //目标点在后面
    {
        v_linear = -std::min(constraint.linear, dist_linear/2);
        //点在右边，后退左转
        if(isRight(state, target)) v_angular = std::min(dist_angular, constraint.angularR);
        else v_angular = -std::min(dist_angular, constraint.angularL);
    }
    UpdateState(v_linear, v_angular);
    // std::cout << number << "x:" << state.x << " y:" << state.y << std::endl;
    // std::cout << "tx:" << average_point.x << " ty:" << average_point.y << std::endl;
}


/**************************************************************************/

//static function init agents
void Agent::InitMultiAgent(std::vector<Agent *> *multi_agent, const std::vector<AgentState> init_states,
                                        const std::vector<double> raduis, const std::vector<double> blind_angles)
{
    if(init_states.size()!=raduis.size() || init_states.size()!=blind_angles.size())
    {
        std::cout << "init error\n";
        return ;
    }
    auto states_it = init_states.begin();
    auto radius_it = raduis.begin();
    auto blind_it = blind_angles.begin();
    int index = 0;
    multi_agent->clear();
    for(; states_it!=init_states.end(); states_it++,radius_it++,blind_it++)
    {
        multi_agent->push_back(new Agent(*states_it, *radius_it, *blind_it, index));
        index++;
    }
}