#include "common.h"
double cross(Point p1, Point p2)
{
    return p1.x*p2.y - p1.y*p2.x;
}

double dot(Point p1, Point p2)
{
    return p1.x*p2.y + p1.y*p2.x;
}

Eigen::Vector2d GetVecFromHead(double head)
{
    double x = cos(head);
    double y = sin(head);
    x = abs(x)<0.000001?0:x; //确保没有计算误差
    y = abs(y)<0.000001?0:y;
    return Eigen::Vector2d(x, y);
}

double GetDist2Bound(AgentState self_state, AgentState neighbor_state, double blind_angle)
{
    double beta, distance_to_boundary;
    Eigen::Vector2d self2n(neighbor_state.x - self_state.x, neighbor_state.y - self_state.y);
    Eigen::Vector2d head =GetVecFromHead(self_state.head);
    beta = acos((-head).dot(self2n)/(head.norm()*self2n.norm()))-blind_angle/2;
    distance_to_boundary = sin(beta) * self2n.norm();

    return distance_to_boundary;
}

bool isRight(AgentState agent, Point p)
{
    Point oringin_point = {agent.x, agent.y};
    Eigen::Vector2d head = GetVecFromHead(agent.head);
    Eigen::Vector2d vector1 = (p - oringin_point).toVector();

    return head.cross(vector1) <= 0;
}

bool isLeft(AgentState agent, Point p)
{
    return !isRight(agent, p);
}

std::string vec2str(std::vector<std::vector<int>> matrix)
{
    std::string str;
    for(int i=0; i<matrix.size(); i++)
    {
        for(int j=0; j<matrix[i].size(); j++)
        {
            str += std::to_string(matrix[i][j]);
            if(j<matrix.size()-1) str += " ";
            else str += "\n";
        }
    }
    return str;
}

std::string vec2str(std::vector<AgentState> matrix)
{
    std::ostringstream oss;
    for(int i=0; i<matrix.size(); i++)
    {
        oss << std::fixed << std::setprecision(2) << matrix[i].x << "  ";
        oss << std::fixed << std::setprecision(2) << matrix[i].y << "  ";
        oss << std::fixed << std::setprecision(2) << matrix[i].head << "\n";
    }
    return oss.str();
}

std::ostream& operator<<(std::ostream& os, const AgentState& as)
{
    os << "x:" << as.x << " y:" << as.y << " head:" << as.head;
    return os;
}