#include <cmath>
#include <iostream>
#include <sstream>
#include <iomanip>
#include "Dense"

struct AgentState {
    double x;
    double y;
    double head;
    AgentState(double init_x=0.0, double init_y=0.0, double init_head=0.0):
                        x(init_x), y(init_y), head(init_head){}
    AgentState(): x(0.0), y(0.0), head(0.0){};
};

struct AgentDynamicData{
    double linear_velocity;
    double angular_velocity;
    AgentDynamicData(double linear_velocity_init, double angular_velocity_init):
        linear_velocity(linear_velocity_init), angular_velocity(angular_velocity_init){}
    AgentDynamicData(): linear_velocity(0.0), angular_velocity(0.0){}
};

struct AgentConstraint{
    double linear;
    double angularL;
    double angularR;
};

struct Point {
    double x;
    double y;
    Point(double x_init, double y_init): x(x_init), y(y_init){}
    Point():x(0.0), y(0.0){}

    //范数
    double norm(){
        return sqrt(x*x + y*y);
    }

    Eigen::Vector2d toVector(){
        return Eigen::Vector2d(x, y);
    }

    /*定义一些操作运算*/
    Point operator+(const Point& other) const{
        return Point(x+other.x, y+other.y);
    }

    Point operator-(const Point& other) const{
        return Point(x-other.x, y-other.y);
    }

    Point& operator+=(const Point& other){
        x = x + other.x;
        y = y + other.y;
        return *this;
    }

    Point& operator-=(const Point& other){
        x = x - other.x;
        y = y - other.y;
        return *this;
    }

    bool operator==(const Point& other) const {
        return (x == other.x) && (y == other.y);
    }

    bool operator!=(const Point& other) const {
        return !(*this == other);
    }
};

double cross(Point p1, Point p2);
double dot(Point p1, Point p2);
Eigen::Vector2d GetVecFromHead(double head);
double GetDist2Bound(AgentState self_state, AgentState neighbor_state, double blind_angle);
bool isLeft(AgentState agent, Point p);
bool isRight(AgentState agent, Point p);
std::string vec2str(std::vector<std::vector<int>>);
std::string vec2str(std::vector<AgentState>);