#include "enviroment.h"

Enviroment::Enviroment(std::vector<Agent *> *agents, double init_lenth, double init_width):
multi_agent(agents), lenth(init_lenth), width(init_width){}

Enviroment::~Enviroment()
{
    /*free memery*/
    for(auto it=multi_agent->begin(); it!=multi_agent->end(); it++)
    {
        delete (*it);
        (*it) = nullptr;
    }
    multi_agent->clear();
}

/*left-right direction of agent*/
bool Enviroment::isRight(Agent* oringin, Agent* agent)
{
    Point oringin_point = oringin->GetPosition();
    Point agent_point = agent->GetPosition();
    Eigen::Vector2d head = oringin->GetHeadVector();
    Eigen::Vector2d vector1 = (agent_point - oringin_point).toVector();


    return head.cross(vector1) < 0;
}

bool Enviroment::isLeft(Agent* oringin, Agent* agent)
{
    Point oringin_point = oringin->GetPosition();
    Point agent_point = agent->GetPosition();
    Eigen::Vector2d head = oringin->GetHeadVector();
    Eigen::Vector2d vector1(agent_point.x - oringin_point.x, agent_point.y - oringin_point.y);

    return head.cross(vector1) > 0;
}

/*-1 left, 1 right, 0 in head*/
int Enviroment::LeftRightDirection(Agent* oringin, Agent* agent)
{
    if(isLeft(oringin, agent))
        return -1;
    else if(isRight(oringin, agent))
        return 1;
    else
        return 0;
}

bool Enviroment::isInBlindAngle(Agent* oringin, Agent* agent)
{
    Eigen::Vector2d oringin_head = oringin->GetHeadVector();
    Eigen::Vector2d agent_vector = (agent->GetPosition() - oringin->GetPosition()).toVector();
    double dot_value = oringin_head.dot(agent_vector);
    double theta = acos(dot_value / (oringin_head.norm()*agent_vector.norm()));

    return theta>(M_PI-oringin->GetBlindAngle()/2.0);
}

AgentState Enviroment::GetRelativeCoordinate(Agent* oringin, Agent* agent)
{
    AgentState coordinate = {agent->GetPosition().x - oringin->GetPosition().x,
                             agent->GetPosition().y - oringin->GetPosition().y,
                             (M_PI/2 - oringin->GetHead()) + agent->GetHead()};
    return coordinate;
}

bool Enviroment::isInSenseRaduis(Agent* oringin, Agent* agent)
{
    double distance = (oringin->GetPosition() - agent->GetPosition()).norm();
    return distance < oringin->GetRaduis();
}

std::vector<std::vector<int>> Enviroment::GetAdjMat()
{
    return adjacency_matrix;
}

std::vector<AgentState> Enviroment::GetCoorMat()
{
    return coordinat_matrix;
}

std::vector<Point> Enviroment::GetTargets()
{
    std::vector<Point> targets;

    for(auto a = multi_agent->begin(); a != multi_agent->end(); a++)
    {
        targets.push_back((*a)->GetAveragePoint());
    }
    return targets;
}

/*observe the enviroment*/
void Enviroment::Observe()
{
    adjacency_matrix.clear();
    coordinat_matrix.clear();
    for(auto observer = multi_agent->begin(); observer != multi_agent->end(); observer++)
    {
        (*observer)->ClearNeighbors();
        std::vector<int> adj_row;
        /*just focus agents in FOV*/
        for(auto agent = multi_agent->begin(); agent != multi_agent->end(); agent++)
        {
            if(isInSenseRaduis(*observer, *agent) && (!isInBlindAngle(*observer, *agent)) && observer!=agent)
           {
                (*observer)->AppendNeighbors((*agent)->GetState());
                adj_row.push_back(1);
           }
           else
           {
                adj_row.push_back(0);
           }
        }

        adjacency_matrix.push_back(adj_row);
        coordinat_matrix.push_back((*observer)->GetState());
    }
}

void Enviroment::ObserveFromData(std::vector<AgentState> states)
{
    if(states.size() != multi_agent->size()) //数量不对就重新new几个
    {
        for(auto agent:*multi_agent) delete agent;
        multi_agent->clear();
        std::vector<double> raduis = {RADUIS, RADUIS, RADUIS, RADUIS};
        std::vector<double> blind_angle = {BLIND_ANGLE, BLIND_ANGLE, BLIND_ANGLE, BLIND_ANGLE};
        Agent::InitMultiAgent(multi_agent, states, raduis, blind_angle);
    }
    else //对的话就重新设置states，节省开销(maybe)
    {
        int i=0;
        for(auto agent:*multi_agent)
        {
            agent->SetState(states[i]);
            i++;
        }
    }

    Observe();
}


void Enviroment::RunOnce()
{
    Observe();

    for(auto agent:*multi_agent)
    {
        agent->MoveAverageStrategy();
    }
}