#pragma once

#include "Dense"
#include "common.h"
#include <vector>

#define DEBUG 0

class Agent
{
private:
    AgentState state;
    double raduis;
    double blind_angle;
    int number;
    Point target;
    std::vector<AgentState> neigbors;
    AgentDynamicData velocity;
    AgentDynamicData velocity_max;
    AgentConstraint constraint;
public:
    Agent(AgentState, double, double, int);
    void UpdateState(double, double);
    void SetState(AgentState);
    AgentState GetState(void);
    Point GetPosition(void);
    double GetBlindAngle(void);
    void AppendNeighbors(AgentState);
    std::vector<AgentState> GetNeighbors(void);
    void ClearNeighbors(void);
    double GetRaduis(void);
    double GetHead(void);
    Eigen::Vector2d GetHeadVector(void);

    bool isInHead(Point p);
    AgentConstraint CalculateConstraint();
    Point GetAveragePoint(void);
    void MoveAverageStrategy(void);

    static void InitMultiAgent(std::vector<Agent *> *,std::vector<AgentState>, std::vector<double>, std::vector<double>);
};