// (c) 2021 Ruggero Rossi
// to build:
//  python3 setup.py build
// to install:
//  python3 setup.py install
// to uninstall it
//  pip uninstall robosoc2d

//#define _R2S_DEBUG

#define PY_SSIZE_T_CLEAN
#include <Python.h>
#include <structmember.h>

#ifndef R2S_DEBUG_PRINT_H
#define R2S_DEBUG_PRINT_H

#include <iostream>

//#define _R2S_DEBUG  // you should not need to #define it: it's definible by CMakeLists.txt
//-On MSVC _DEBUG is already defined in debug compilings
//-On CMAKE _R2S_DEBUG is set in this project's CMakeLists.txt to be automatically #defined in debug compilings
//-if not using either CMAKE or MSVC, you should manually #define it in debug compilings, or "g++ -D _R2S_DEBUG"

#if defined(_DEBUG) || defined(_R2S_DEBUG)  
#define DEBUG_OUT(x) do { std::cout << x; } while (0)
#define DEBUG_ERR(x) do { std::cerr << x; } while (0)

template <typename T> // useful for debugging, just like in Scott Meyers' Effective Modern C++, item 4
void tellType(const T& param)
{
  std::cout << "T =   " << typeid(T).name() << "\n";
  std::cout << "param =   " << typeid(param).name() << "\n";
}

#else
#define DEBUG_OUT(x)
#define DEBUG_ERR(x)
#define tellType(x)
#endif

#endif // DEBUG_PRINT_H 

// (c) 2021 Ruggero Rossi
// very simple 2D vector inline class for robosoc2d
#ifndef VEC2_H
#define VEC2_H

#ifdef _WIN32
#include  <numeric>
//#define __WXMSW__
//#define _UNICODE
//#define NDEBUG
#else // __linux__ 
#endif

#define _USE_MATH_DEFINES
#include <cmath>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

namespace r2s {

struct Vec2 {
	double x, y;
public:
	Vec2() : x(0), y(0) {}
	Vec2(double x, double y) : x(x), y(y) {}
	Vec2(const Vec2& v) : x(v.x), y(v.y) {}
	Vec2(double angle) : x(cos(angle)), y(sin(angle)) {}
	Vec2& operator=(const Vec2& v) { x = v.x; y = v.y; return *this; }
	void set(double vx, double vy) { x = vx; y = vy; }
	void zero() { x=0.0; y=0.0; }
	void invert() { x=-x; y=-y;}
	Vec2 operator+(const Vec2& v) { return Vec2(x + v.x, y + v.y); }
	Vec2 operator-(const Vec2& v) { return Vec2(x - v.x, y - v.y); }
	Vec2& operator+=(const Vec2& v) { x += v.x; y += v.y; return *this; }
	Vec2& operator-=(const Vec2& v) { x -= v.x; y -= v.y; return *this; }
	Vec2& add(double vx, double vy) { x += vx; y += vy; return *this; }
	Vec2& sub(double vx, double vy) { x -= vx; y -= vy; return *this; }
	Vec2 operator*(double k) { return Vec2(x * k, y * k); }
	Vec2 operator/(double k) { return Vec2(x / k, y / k); }
	Vec2& operator*=(double k) { x *= k; y *= k; return *this; }
	Vec2& operator/=(double k) { x /= k; y /= k; return *this; }
	double len() const { return sqrt(x * x + y * y); }

	void norm() {
		double l = len();
		if (l != 0.0) {
			double k = 1.0 / l;
			x *= k;
			y *= k;
		}
	}
	void resize(double length) { norm(); x *= length; y *= length; }
	double dist(const Vec2& v) const { Vec2 d(v.x - x, v.y - y); return d.len(); }

	void rot(double rad) {
		double c = cos(rad);
		double s = sin(rad);
		double rx = x * c - y * s;
		double ry = x * s + y * c;
		x = rx;
		y = ry;
	}

	void rotDeg(double deg) { double rad = deg / 180.0 * M_PI; rot(rad); }

	double dot(const Vec2& v) { return x * v.x + y * v.y; }
	double cosBetween(const Vec2& v) { double l= len()*v.len(); return (l==0.0 ? 0.0 : dot(v)/l); }	// cosine of the angle between the two vectors
};

} // end namespace
#endif // VEC2_H

// (c) 2021 Ruggero Rossi
// robosoc2d : a Very Simplified 2D Robotic Soccer Simulator
#ifndef R2S_SIMULATOR_H
#define R2S_SIMULATOR_H

#ifdef _WIN32
    #include  <numeric>
    //#define __WXMSW__
    //#define _UNICODE
    //#define NDEBUG
#else // __linux__ 
#endif

#include <iostream>
#include <string>
#include <memory>
#include <vector>
#include <random>
#include <array>
#include <set>
#include <limits>
#include <chrono>

#define _USE_MATH_DEFINES
#include <cmath>

#ifndef M_PI
    #define M_PI 3.14159265358979323846
#endif
#ifndef M_PI_2
    #define M_PI_2 1.57079632679489661923
#endif
#ifndef M_PI_4
    #define M_PI_4 0.785398163397448309616
#endif

namespace r2s { 

constexpr auto R2SVersion="1.0.0";
inline const char* GetR2SVersion(){return R2SVersion;};

constexpr auto defaultTeam1Name = "Team A";
constexpr auto defaultTeam2Name = "Team B";

constexpr double ContemporaryKickProbability=0.5;
constexpr double R2SmallEpsilon=std::numeric_limits<double>::epsilon();
constexpr double R2Epsilon=std::numeric_limits<double>::epsilon() * 10.0;
constexpr double R2BigEpsilon=std::numeric_limits<double>::epsilon() * 100.0;

constexpr double BallPlayerHitFactor=1.1;   //to avoid ball having exactly the same speed 
constexpr double BallPlayerHitFactorSimplified=1.001;   //to avoid ball having exactly the same speed (use 0.0 to have it completely stopped)
constexpr double BallPoleBounceFactor=0.95; // 0.95;
constexpr double BallPlayerBounceFactor=0.91;
constexpr double BallPlayerStopFactor=0.25;
constexpr double CollisionPlayerDisplaceFactor=0.2;

/*
constexpr double PlayerVelocityDecay=0.7; //0.7 big - 09 small;
constexpr double BallVelocityDecay=0.95; //0.95 big - 0.97 small;
constexpr double MaxPlayerSpeed=1.111; //1.111 big - 0.1111 small; // Bolt goes to more than 44 km/h. Let's say a good speed could be 40 km/h. It's 40m every 3,6 seconds, that's 11,111 m/s, that's 1,111 m/tick with 10 ticks per second.
constexpr double MaxBallSpeed=1.9; //1.9 big - 0.19 small; // 3 is almost 100 km/h
constexpr double MaxDashPower=0.2;//0.2 big - 0.02 small;
constexpr double MaxKickPower=1.5;//1.5 big - 0.15 small;
constexpr double KickRadius=0.2;//0.2 big - 0.1 small;
constexpr double BallRadius=0.3;    // 0.3 big - 0.11 small
constexpr double PlayerRadius=0.8;  // 0.8 big - 0.4 small
*/

constexpr double PlayerVelocityDecay=0.9; //0.7 big - 09 small;
constexpr double BallVelocityDecay=0.97; //0.95 big - 0.97 small;
constexpr double MaxPlayerSpeed=0.2222; //1.111 big - 0.1111 small; // Bolt goes to more than 44 km/h. Let's say a good speed could be 40 km/h. It's 40m every 3,6 seconds, that's 11,111 m/s, that's 1,111 m/tick with 10 ticks per second.
constexpr double MaxBallSpeed=0.6; //1.9 big - 0.19 small; // 3 is almost 100 km/h
constexpr double MaxDashPower=0.06;//0.2 big - 0.02 small;
constexpr double MaxKickPower=0.65;//1.5 big - 0.15 small; // old defaul 0.45
constexpr double KickRadius=0.1;//0.2 big (old standard) - 0.1 small; in addition to player radius
constexpr double BallRadius=0.11;    // 0.3 big - 0.11 small
constexpr double PlayerRadius=0.4;  // 0.8 big - 0.4 small

constexpr double PlayerRandomNoise=0.005;//=0.2;
constexpr double PlayerDirectionNoise=0.005;//=0.2;
constexpr double PlayerVelocityDirectionMix=0.2;//=0.2;
constexpr double BallInsidePlayerVelocityDisplace=0.5;//=0.2;
constexpr double AfterCatchDistance=0.05;
constexpr int   CatchHoldingTicks=2;
constexpr double PoleRadius=0.055; 

constexpr double CatchProbability=0.9;
constexpr double CatchRadius=0.3;
constexpr double KickableAngle= M_PI/3.0; //kickable angle between player direction and ball-player vector
constexpr double KickableDirectionAngle= M_PI_2; //kickable angle between player direction and direction of the kick
constexpr double CatchableAngle= M_PI_2; //
constexpr double CosCatchableAngle=0.0; // M_PI/3

constexpr double RegularPitchLength=105.0;
constexpr double RegularPitchWidth=68.0;
constexpr double RegularAreaLength=16.5;
constexpr double RegularAreaWidth=40.32;
constexpr double RegularGoalWidth=7.32;
constexpr double RegularCornerDistance=9.15;    // minimum distance of opponent when kicking corner
constexpr double MinimumCornerDistance=2.5; // this is typical for 5-a-side soccer = 4.0
constexpr double MinimumThrowinDistance=2.5; // this is typical for 5-a-side soccer = 4.0
constexpr int   MaxCollisionLoop=10;    // 10
constexpr int   MaxCollisionInsideTickLoop=40;  //40
constexpr double PlayerOutOfPitchLimit=3.0;

inline double calcAreaLength(double pitchLength) { return pitchLength/RegularPitchLength*RegularAreaLength; }
inline double calcAreaWidth(double pitchWidth) { return pitchWidth/RegularPitchWidth*RegularAreaWidth; }
inline double calcCornerDistance(double pitchWidth) { double d=pitchWidth/RegularPitchWidth*RegularCornerDistance; return (d<MinimumCornerDistance)? MinimumCornerDistance : d; }

inline unsigned int createChronoRandomSeed(){ return std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::high_resolution_clock::now().time_since_epoch()).count(); } 

//sets angle between 0 and 2 PI
double fixAnglePositive(double angle);
//sets angle between + and - PI
double fixAngleTwoSides(double angle);

struct R2Pitch {
    double x1,x2,y1,y2;
    double xGoal1, xGoal2;
    double yGoal1, yGoal2;
    double areaLx, areaRx, areaUy, areaDy;
    double goalKickLx, goalKickRx, goalKickUy, goalKickDy;
    Vec2 poles[4];
    double border_up, border_down, border_left, border_right;

    R2Pitch(double _pitchWidth=RegularPitchWidth, double _pitchLength=RegularPitchLength, double _goalWidth=RegularGoalWidth,
     double _netLength=0.0, double _poleRadius=PoleRadius, double _borderLimit=PlayerOutOfPitchLimit):
        x1(_pitchLength/2.0), x2(-x1),
        y1(_pitchWidth/2.0), y2(-y1), 
        xGoal1(x1+_netLength), xGoal2( -xGoal1 ),
        yGoal1(_goalWidth/2.0), yGoal2( -yGoal1 ),
        areaLx(x2+calcAreaLength(_pitchLength)), 
        areaRx(x1-calcAreaLength(_pitchLength)), 
        areaUy(calcAreaWidth(_pitchWidth)/2.0),
        areaDy(-calcAreaWidth(_pitchWidth)/2.0), 
        goalKickLx(x2+(areaLx-x2)/2.0),  
        goalKickRx(x1+(areaRx-x1)/2.0),
        goalKickUy(yGoal1+(areaUy-yGoal1)/2.0),
        goalKickDy(yGoal2+(areaDy-yGoal2)/2.0),
        poles{Vec2(x2, yGoal1+_poleRadius), Vec2(x2, yGoal2-_poleRadius), Vec2(x1, yGoal1+_poleRadius), Vec2(x1, yGoal2-_poleRadius)},
        border_up(y1+_borderLimit), border_down(y2-_borderLimit), border_left(x2-_borderLimit), border_right(x1+_borderLimit)  {}
};
struct R2EnvSettings {
    bool simplified;
    int ticksPerTime;
    double pitchLength;
    double pitchWidth;
    double goalWidth;
    double centerRadius;
    double poleRadius;   
    double ballRadius;
    double playerRadius;
    double catchRadius;
    int catchHoldingTicks;
    double kickRadius;
    double kickableDistance;
    double catchableDistance;
    double kickableAngle;
    double kickableDirectionAngle;
    double catchableAngle;
    double netLength;
    double catchableAreaLength;
    double catchableAreaWidth;
    double cornerMinDistance;
    double throwinMinDistance;
    double outPitchLimit;
    double maxDashPower;
    double maxKickPower;
    double playerVelocityDecay;
    double ballVelocityDecay;
    double maxPlayerSpeed;
    double maxBallSpeed;
    double catchProbability;
    double playerRandomNoise;
    double playerDirectionNoise;
    double playerVelocityDirectionMix;
    double ballInsidePlayerVelocityDisplace;
    double afterCatchDistance;

    /**
        with 11 players: 105x68 with 7.32 mts goal, area 16,5x40,32, central circle 9,15m
        with 5 players: 40x24 with 4 mts goal, center circle 3m radius, area (proportion) 
        with 4 players: 32x19.2 with 4 mts goal, center circle 2,4m radius
    */
    R2EnvSettings(bool _simplified=true, double _pitchLength=32, double _pitchWidth=19.2, double _goalWidth=4.0, double _centerRadius=2.4,
    int _ticksPerTime=3000, double _poleRadius=PoleRadius,  double _ballRadius=BallRadius, double _playerRadius=PlayerRadius, double _catchRadius=CatchRadius, double _kickRadius=KickRadius,
    double _kickableAngle=KickableAngle, double _kickableDirectionAngle=KickableDirectionAngle, double _catchableAngle=CatchableAngle,  double _netLength=1.5) :
        simplified(_simplified), ticksPerTime(_ticksPerTime), pitchLength(_pitchLength), pitchWidth(_pitchWidth), goalWidth(_goalWidth), 
        centerRadius(_centerRadius), poleRadius(_poleRadius), ballRadius(_ballRadius), playerRadius(_playerRadius), catchRadius(_catchRadius), catchHoldingTicks(CatchHoldingTicks),
        kickRadius(_kickRadius), kickableDistance(kickRadius+playerRadius+ballRadius), catchableDistance(catchRadius+playerRadius+ballRadius),
        kickableAngle(_kickableAngle), kickableDirectionAngle(_kickableDirectionAngle),
        catchableAngle(_catchableAngle), netLength(_netLength),
        catchableAreaLength(calcAreaLength(_pitchLength)), catchableAreaWidth(calcAreaWidth(_pitchWidth)),
        cornerMinDistance(calcCornerDistance(_pitchWidth)), throwinMinDistance(MinimumThrowinDistance), outPitchLimit(PlayerOutOfPitchLimit),
        maxDashPower(MaxDashPower), maxKickPower(MaxKickPower),
        playerVelocityDecay(PlayerVelocityDecay), ballVelocityDecay(BallVelocityDecay), maxPlayerSpeed(MaxPlayerSpeed), maxBallSpeed(MaxBallSpeed),
        catchProbability(CatchProbability), playerRandomNoise(PlayerRandomNoise), playerDirectionNoise(PlayerDirectionNoise), playerVelocityDirectionMix(PlayerVelocityDirectionMix),
        ballInsidePlayerVelocityDisplace(BallInsidePlayerVelocityDisplace), afterCatchDistance(AfterCatchDistance)   {}    
};

enum class R2State {
    Inactive, //!< not active at all
    Ready,    //!< UNUSED 
    Kickoff1,  //!< kickoff (beginning or after a goal), team 1 (left)
    Kickoff2,
    Play,     //!< game is playing
    Stopped,  //!< UNUSED - game is stopped
    Goalkick1up,   //!< goal-kick team 1 (left)
    Goalkick1down,   //!< goal-kick team 1 (left)
    Goalkick2up,   //!< goal-kick
    Goalkick2down,   //!< goal-kick
    Corner1up,
    Corner1down,
    Corner2up,
    Corner2down,
    Throwin1,  //throw-in
    Throwin2,  
    Paused,   //!< UNUSED - simulation paused
    Halftime, //!< half time interval (if any, not sure it ever reaches this state)
    Goal1,     //!< Team1 scored a goal
    Goal2,     //!< Team2 scored a goal
    Ended     //!< end of the match
};

struct R2ObjectInfo {
    Vec2 pos, velocity;

    R2ObjectInfo(double _x=0.0, double _y=0.0, double _xVelocity=0.0, double _yVelocity=0.0):
        pos(_x, _y), velocity(_xVelocity, _yVelocity) {}

    double absVelocity() { return velocity.len(); };
    double absDistanceFromCenter() { return pos.len(); };

    std::pair<double, Vec2> dist(R2ObjectInfo& obj){
        Vec2 d = obj.pos - pos;
        return std::pair<double, Vec2> { d.len(), d};
    }
};

struct R2PlayerInfo : R2ObjectInfo {
    double direction; //<! where's pointing the front of the player. In radiants. 0.0 points towards right of the field.
    bool acted; //if it already acted during this tick
    R2PlayerInfo(double _x=0.0, double _y=0.0, double _xVelocity=0.0, double _yVelocity=0.0, double _direction=0.0) : R2ObjectInfo(_x, _y, _xVelocity, _yVelocity) , direction(_direction), acted(false) {}
};

struct R2Environment {
    int tick;    //!<current tick
    int score1;
    int score2;
    R2State state;
    R2ObjectInfo ball;
    std::vector<R2PlayerInfo> teams[2];

    bool lastTouchedTeam2;  
    double startingTeamMaxRange;    // max movement done by the team that is starting the kick (goal kick, throwin, corner)
    int ballCatched;        // if >0 the ball is catched and only the goalkeeper can kick it and the ball moves with him. The value indicates how many ticks long the goalkeeper can hold the ball.
    bool ballCatchedTeam2;  // if ballis catched, this says if its catched by Team2 goalkeeper
    bool halftimePassed;

    R2Environment(int nPlayers1=0, int nPlayers2=0) :
        tick(0), score1(0), score2(0), state(R2State::Inactive), ball(), 
        teams{std::vector<R2PlayerInfo>(nPlayers1),std::vector<R2PlayerInfo>(nPlayers2)},
        lastTouchedTeam2(false), startingTeamMaxRange(0.0), ballCatched(0), ballCatchedTeam2 (false), halftimePassed(false) {}
};

struct R2GameState {
    R2EnvSettings sett;
    R2Environment env;
    R2Pitch pitch;
    R2GameState(R2EnvSettings _sett, R2Environment _env, R2Pitch _pitch) : sett(_sett), env(_env), pitch(_pitch) {}
};

enum class R2ActionType {
    NoOp,   //<! don't do anything
    Move,   //<! position on the pitch
    Dash,   //<! accelerate
    Turn,   //<! UNUSED
    Kick,
    Catch   //<! only goalkeeper, in area
};

struct R2Action {
    R2ActionType action;
    double data[3];

    R2Action(R2ActionType actionType=R2ActionType::NoOp, double data1=0.0, double data2=0.0, double data3=0.0):
        action(actionType), data{data1, data2, data3} {}
};

class R2Player {
public:
    virtual R2Action step(const R2GameState gameState) = 0;
    virtual ~R2Player() = default; 
};

std::tuple<int, double, double> intersectionSegmentCircle(Vec2 s1, Vec2 s2, Vec2 c1, double r);

struct R2ActionRecord{
    int team;
    int player;
    R2Action action;

    R2ActionRecord(int _team=0, int _player=0, R2Action _action=R2Action()):
        team(_team), player(_player), action(_action) {}
};

struct R2History{
std::vector<R2Environment> envs;
std::vector<std::vector<R2ActionRecord>> actions;  // is team 2, player, action

R2History(int ticks, int nplayers1, int nplayers2) : envs(ticks+1), actions(ticks, std::vector<R2ActionRecord>(nplayers1+nplayers2) ){};
};

struct R2BallPlayerCollision{
    double t;
    int p;
    int team;
    R2BallPlayerCollision(double _t, int _p, int _team) : 
        t(_t), p(_p), team(_team) {}
};

struct R2PoleBallCollision{
    bool collision;
    double t;
    int pole;   
    R2PoleBallCollision(bool _collision, double _t, int _pole) : 
        collision(_collision), t(_t), pole(_pole) {}
};

struct R2PolePlayerCollision{
    double t;
    int p;
    int team;
    int pole;
    R2PolePlayerCollision(double _t, int _p, int _team, int _pole) : 
        t(_t), p(_p), team(_team), pole(_pole) {}
};

struct R2PlayerPlayerCollision{
    double t;
    int p1;
    int team1;
    int p2;
    int team2;

    R2PlayerPlayerCollision(double _t, int _p1, int _team1, int _p2, int _team2) : 
        t(_t), p1(_p1), team1(_team1), p2(_p2), team2(_team2) {}
};

enum class R2CollisionType {
    None,
    PoleBall,
    PolePlayer,
    BallPlayer,
    PlayerPlayer
};

struct R2CollisionTime{
    double t;
    R2CollisionType type;
    R2CollisionTime(double _t, R2CollisionType _type): t(_t), type(_type){}
};

class R2Simulator{
private:
        R2EnvSettings sett;
        R2Environment env, oldEnv;
        R2Pitch pitch;
        unsigned int random_seed;
        std::default_random_engine rng;
        std::normal_distribution<double> normalDist;
        std::uniform_real_distribution<double> uniformDist;
        std::vector<std::shared_ptr<R2Player>> teams[2];
        std::vector<int> shuffledPlayers;
        bool startedTeam2;
        bool ballAlreadyKicked;
        const std::set<R2State> notStarterStates;
        const std::set<R2State> team2StarterStates;
        R2History history;
        int processedActions;
        std::string teamNames[2];
        double cosKickableAngle;
        double cosCatchableAngle;

        bool isBallOutUp(){return (env.ball.pos.y > pitch.y1);}
        bool isBallOutDown(){return (env.ball.pos.y < pitch.y2);}
        bool isBallOutLeft(){return (env.ball.pos.x < pitch.x2);}
        bool isBallOutRight(){return (env.ball.pos.x > pitch.x1);}
        bool isBallOut(){return (isBallOutUp()||isBallOutDown()||isBallOutLeft()||isBallOutRight());}
        
        void resetPlayersActed();
        void setBallThrowInPosition();
        bool isBallInGoal(const int team);  // Team is the one scoring
        bool isGoalScored(const int team);
        bool didBallIntersectGoalLine(const int team);
        bool isPlayerOut(const int player, const int team);
        bool isPlayerInsideHisArea(const int player, const int team);
        bool isPlayerInsideOpponentArea(const int player, const int team);
        bool isPlayerInsideOpponentAreaFullBody(Vec2 pos, const int team);
        bool isPlayerInsideOpponentAreaFullBody(const int player, const int team);
        void limitBallSpeed();
        void limitPlayerSpeed(R2PlayerInfo& p);
        void limitSpeed();
        void decayPlayerSpeed(R2PlayerInfo& p);
        void decaySpeed();
        void putPlayersFarFromBall(int team, double minDist);
        Vec2 avoidOtherPlayersPosition(Vec2 pos, int team, int player);
        void actionMove(const R2Action& action, int team, int player);
        void actionMoveKickoff(const R2Action& action, int team, int player);
        void actionMoveGoalkick(const R2Action& action, int team, int player);
        void actionMoveThrowinCorner(const R2Action& action, int team, int player, double distanceToBall);
        void actionMoveThrowin(const R2Action& action, int team, int player);
        void actionMoveCorner(const R2Action& action, int team, int player);
        void actionDash(const R2Action& action, int team, int player);
        void actionKick(const R2Action& action, int team, int player);
        void actionCatch(const R2Action& action, int team, int player);
        void setBallCatchedPosition();
        void setBallReleasedPosition();
        void limitPlayersCloseToPitch();
        void limitPlayersToHalfPitch(int kickTeam, double dueDistance);
        void limitPlayersToHalfPitchCenterBody(int kickTeam){limitPlayersToHalfPitch(kickTeam, sett.centerRadius);}
        void limitPlayersToHalfPitchFullBody(int kickTeam){limitPlayersToHalfPitch(kickTeam, sett.centerRadius+sett.playerRadius);}
        void limitPlayersOutsideArea(int kickTeam);
        void limitPlayersOutsideAreaFullBody(int kickTeam);
        bool checkBallOut();
        bool checkGoalOrBallOut();
        void processStep(const R2Action& action, const int team, const int player);
        void procReady(const R2Action& action, const int team, const int player);
        void procKickoff(const R2Action& action, const int team, const int player);
        void procPlay(const R2Action& action, const int team, const int player);
        void procGoalkick(const R2Action& action, const int team, const int player);
        void procCorner(const R2Action& action, const int team, const int player);
        void procThrowin(const R2Action& action, const int team, const int player);
        void procGoal(const R2Action& action, const int team, const int player);
        void procEnded(const R2Action& action, const int team, const int player);
        void preState();
        void checkState();
        std::tuple<bool, double> findPoleObjectCollision(R2ObjectInfo& obj1, Vec2 pole, double radius, double partialT);
        std::tuple<bool, double> findObjectsCollision(R2ObjectInfo& obj1, R2ObjectInfo&obj2, double radius, double partialT);

        std::tuple<bool, double> findBallPlayerCollision(int team, int player, double partialT);
        std::vector<R2BallPlayerCollision> findFirstBallPlayersCollisions(double partialT, std::vector<bool>& ballPlayerBlacklist);
        std::tuple<bool, double> findPlayerPlayerCollision(int team1, int player1, int team2, int player2, double partialT);
        std::vector<R2PlayerPlayerCollision> findFirstPlayerPlayersCollisions(double partialT, std::vector<int>& playerPlayerCollisions);
       
        std::tuple<bool, double> findPoleBallCollision(Vec2 pole, double partialT);
        std::tuple<bool, double> findPolePlayerCollision(int team, int player, Vec2 pole, double partialT);

        R2PoleBallCollision findFirstPoleBallCollision(double partialT);
        std::vector<R2PolePlayerCollision> findFirstPolePlayersCollisions(double partialT);


        void addBallNoise();
        void updateMotion(double t);
        void manageCollisions();
        bool manageStaticPoleBallCollisions();
        void manageStaticBallCollisions();
        bool manageStaticPolePlayersCollisions();
        void manageStaticPlayersCollisions();
        void updateCollisionsAndMovements();
        bool isAnyTeamPreparingKicking();
        bool isAnyTeamKicking();
        bool isTeam2Kicking(R2State theState);
        void playersAct();

        void manageBallInsidePlayers();
public:
    R2Simulator(std::vector<std::shared_ptr<R2Player>> _team1, std::vector<std::shared_ptr<R2Player>> _team2, std::string _team1name=defaultTeam1Name, std::string _team2name=defaultTeam2Name,
      unsigned int _random_seed = createChronoRandomSeed() ,
      R2EnvSettings _settings = R2EnvSettings() ) :
        sett(_settings) , env(_team1.size(), _team2.size()) ,  oldEnv(_team1.size(), _team2.size()),
        pitch(_settings.pitchWidth, _settings.pitchLength, _settings.goalWidth, _settings.netLength, _settings.poleRadius, _settings.outPitchLimit),
        random_seed (_random_seed),
        rng (_random_seed),
        normalDist(), uniformDist(0.0, 1.0),
        teams{_team1, _team2} ,
        shuffledPlayers(_team1.size()+_team2.size(),0),
        startedTeam2(false),
        ballAlreadyKicked(false),
        notStarterStates({R2State::Inactive, R2State::Ready, R2State::Play, R2State::Stopped, R2State::Paused, R2State::Halftime, R2State::Goal1, R2State::Goal2, R2State::Ended}),
        team2StarterStates({R2State::Kickoff2, R2State::Goalkick2up, R2State::Goalkick2down, R2State::Corner2up, R2State::Corner2down, R2State::Throwin2}),
        history(_settings.ticksPerTime*2, _team1.size(), _team2.size()),
        processedActions(0),
        teamNames{_team1name ,_team2name},
        cosKickableAngle (cos(sett.kickableAngle)),
        cosCatchableAngle(cos(sett.catchableAngle))
        {
            std::iota (std::begin(shuffledPlayers), std::end(shuffledPlayers), 0);
        }

    void setStartMatch();
    void setHalfTime();
    void playMatch();
    void step();
    bool stepIfPlaying();
    R2GameState getGameState() { return R2GameState(sett, env, pitch); };
    std::vector<std::string> getTeamNames();
    std::string getStateString();
    unsigned int getRandomSeed() { return random_seed;};
    std::string createDateFilename();
    bool saveStatesHistory(std::string filename);
    bool saveStatesHistory(){ return saveStatesHistory(createDateFilename().append(".states.txt")); }
    bool saveActionsHistory(std::string filename);
    bool saveActionsHistory(){ return saveActionsHistory(createDateFilename().append(".actions.txt")); }
    bool saveHistory(){ std::string fn1,fn2=createDateFilename(); fn1=fn2; bool r=saveStatesHistory(fn1.append(".states.txt")); return ( saveActionsHistory(fn2.append(".actions.txt")) && r);}

    void setEnvironment(int _tick, int _score1, int _score2, R2State _state, R2ObjectInfo _ball, 
        std::vector<R2PlayerInfo> _team1, std::vector<R2PlayerInfo> _team2,
        bool _lastTouchedTeam2, int _ballCatched, bool _ballCatchedTeam2) ;
};

template <typename player> 
std::vector<std::shared_ptr<R2Player>> buildTeam(int nPlayers, int whichTeam){
    std::vector<std::shared_ptr<R2Player>> team;
    static_assert(std::is_base_of<R2Player, player>::value, "Class of player not derived from R2Player");
    for (int i=0 ; i <nPlayers; i++){
        team.push_back(std::static_pointer_cast<R2Player>(std::make_shared<player>(i, whichTeam)));
    }
    return team;
}

template <typename goalkeeper, typename players> 
std::vector<std::shared_ptr<R2Player>> buildTeam(int nPlayers, int whichTeam){
    std::vector<std::shared_ptr<R2Player>> team;
    static_assert(std::is_base_of<R2Player, goalkeeper>::value, "Class of goalkeeper not derived from R2Player");
    static_assert(std::is_base_of<R2Player, players>::value, "Class of player not derived from R2Player");

    if(nPlayers>0)
        team.push_back(std::static_pointer_cast<R2Player>(std::make_shared<goalkeeper>(0, whichTeam)));

    for (int i=1 ; i <nPlayers; i++){
        team.push_back(std::static_pointer_cast<R2Player>(std::make_shared<players>(i, whichTeam)));
    }
    return team;
}

template<typename team1Player, typename team2Player>
std::unique_ptr<R2Simulator> buildSimulator(int nPlayers1, int nPlayers2, std::string team1name=defaultTeam1Name, std::string team2name=defaultTeam2Name, 
        unsigned int random_seed = createChronoRandomSeed() ,
        R2EnvSettings settings = R2EnvSettings() ){
    std::vector<std::shared_ptr<R2Player>> team1, team2;
    team1= buildTeam<team1Player>(nPlayers1,0);
    team2= buildTeam<team2Player>(nPlayers2,1);
    return std::make_unique<R2Simulator>(team1, team2, team1name, team2name, random_seed, settings);
}

template<typename team1Goalkeeper, typename team1Player, typename team2goalkeeper, typename team2Player>
std::unique_ptr<R2Simulator> buildSimulator(int nPlayers1, int nPlayers2, std::string team1name=defaultTeam1Name, std::string team2name=defaultTeam2Name, 
        unsigned int random_seed = createChronoRandomSeed() ,
        R2EnvSettings settings = R2EnvSettings()){
    std::vector<std::shared_ptr<R2Player>> team1, team2;
    team1= buildTeam<team1Goalkeeper,team1Player>(nPlayers1,0);
    team2= buildTeam<team2goalkeeper,team2Player>(nPlayers2,1);
    return std::make_unique<R2Simulator>(team1, team2, team1name, team2name, random_seed, settings);
}

template<typename player>
std::unique_ptr<R2Simulator> buildOneTeamSimulator(int nPlayers, int teamNumber, std::vector<std::shared_ptr<R2Player>> otherTeam,
        std::string team1name=defaultTeam1Name, std::string team2name=defaultTeam2Name, 
        unsigned int random_seed = createChronoRandomSeed() ,
        R2EnvSettings settings = R2EnvSettings()){ 
    std::vector<std::shared_ptr<R2Player>>  team= buildTeam<player>(nPlayers, teamNumber);
    return teamNumber ? std::make_unique<R2Simulator>(otherTeam, team, team1name, team2name, random_seed, settings) : std::make_unique<R2Simulator>(team, otherTeam, team1name, team2name, random_seed, settings);
}

template<typename goalkeeper, typename player>
std::unique_ptr<R2Simulator> buildOneTeamSimulator(int nPlayers, int teamNumber, std::vector<std::shared_ptr<R2Player>> otherTeam,
    std::string team1name=defaultTeam1Name, std::string team2name=defaultTeam2Name, 
        unsigned int random_seed = createChronoRandomSeed() ,
    R2EnvSettings settings = R2EnvSettings()){ 
    std::vector<std::shared_ptr<R2Player>>  team= buildTeam<goalkeeper, player>(nPlayers, teamNumber);
    return teamNumber ? std::make_unique<R2Simulator>(otherTeam, team, team1name, team2name, random_seed, settings) : std::make_unique<R2Simulator>(team, otherTeam, team1name, team2name, random_seed, settings);
}

} // end namespace
#endif // SIMULATOR_H

// (c) 2021 Ruggero Rossi
// robosoc2d : a Very Simplified 2D Robotic Soccer Simulator
#ifdef _WIN32
  #include <numeric>
#else // __linux__ 
#endif

#include <vector>
#include <random>
#include <algorithm>
#include <cmath>
#include <iostream>
#include <fstream>
#include <ctime>

using namespace std;

namespace r2s {

//sets angle between 0 and 2 PI
double fixAnglePositive(double angle){
  double angle2=remainder(angle, 2*M_PI);
  if(angle2< 0.0)
    angle2= 2*M_PI + angle2;
  return angle2;
}

//sets angle between + and - PI
double fixAngleTwoSides(double angle){
  double angle2=remainder(angle, 2*M_PI);
  //if(angle2< -M_PI) // it would never enter this condition because remainder returns the remaind to the number=quotient*divisor that is closer to dividend, so it will not be, in absolute terms, bigger than divisor/2  
  //  angle2= 2*M_PI + angle2;
  //if(angle2> M_PI)  // for the same reason it would never enter this condition as well. Different kind of "remainder" or "module" functions would need those conditions though.
  //  angle2= -2*M_PI + angle2;
  return angle2;
}

void R2Simulator::limitPlayersCloseToPitch(){
  for(int w=0; w<=1; w++)
    for(int n=0; n< env.teams[w].size(); n++){
      auto& p= env.teams[w][n];
      if(p.pos.x < pitch.border_left)
        p.pos.x = pitch.border_left;
      else if(p.pos.x > pitch.border_right)
        p.pos.x = pitch.border_right;

      if(p.pos.y < pitch.border_down)
        p.pos.y = pitch.border_down;
      else if(p.pos.y > pitch.border_up)
        p.pos.y = pitch.border_up;
    }
}

void R2Simulator::setBallThrowInPosition(){
  double borderY= (env.ball.pos.y > 0.0) ? pitch.y1 : pitch.y2;
  double intersectionX=0.0;
  Vec2 delta=env.ball.pos-oldEnv.ball.pos;
  if(fabs(delta.y)>R2BigEpsilon){
    double m=delta.x/delta.y;
    intersectionX=oldEnv.ball.pos.x + m*(borderY-oldEnv.ball.pos.y);
  }
  else{
    intersectionX=(oldEnv.ball.pos.x+env.ball.pos.x)/2;
  }
  env.ball.pos.x=intersectionX;
  env.ball.pos.y=borderY;
}

bool R2Simulator::isBallInGoal(const int team){ // team : in teams[team] own goal 
  if( (team && (!env.halftimePassed) ) || ( (!team) && env.halftimePassed) ) // score in Team2 own goal
    return ( (env.ball.pos.x > pitch.x1) && (env.ball.pos.x < pitch.xGoal1) && (env.ball.pos.y > pitch.yGoal2) && (env.ball.pos.y < pitch.yGoal1) );
  else
    return ( (env.ball.pos.x > pitch.xGoal2) && (env.ball.pos.x < pitch.x2) && (env.ball.pos.y > pitch.yGoal2) && (env.ball.pos.y < pitch.yGoal1) );
}

bool R2Simulator::didBallIntersectGoalLine(const int team){
  if((team && (!env.halftimePassed) ) || ( (!team) && env.halftimePassed)){ // score in Team2 own goal
    if(env.ball.pos.x < pitch.x1)
      return false;
  }
  else{
    if(env.ball.pos.x > pitch.x2)
      return false;
  }

  double goalX= ( (team && (!env.halftimePassed) ) || ( (!team) && env.halftimePassed) )? pitch.x1 : pitch.x2;
  Vec2 delta=env.ball.pos-oldEnv.ball.pos;
  if(delta.x!=0.0){
    double m=delta.y/delta.x;
    double intersectionY=oldEnv.ball.pos.y + m*(goalX-oldEnv.ball.pos.x);
    if((intersectionY > pitch.yGoal2) &&(intersectionY < pitch.yGoal1)){
      return true;
    }
  }
  return false;
}

bool R2Simulator::isGoalScored(int team){ 
  if(didBallIntersectGoalLine(team)){
    return true;
  }
  // if it entered the goal passing through an external path, reposition the ball in an outside zone
  if(isBallInGoal(team)){
    double goalX= ( (team && (!env.halftimePassed) ) || ( (!team) && env.halftimePassed) ) ? pitch.x1 : pitch.x2;
    double epsilonOut= ( (team && (!env.halftimePassed) ) || ( (!team) && env.halftimePassed) ) ? R2Epsilon : -R2Epsilon;
    Vec2 delta=env.ball.pos-oldEnv.ball.pos;
    if(delta.x!=0.0){
      double m=delta.y/delta.x;
      double intersectionY=oldEnv.ball.pos.y + m*(goalX-oldEnv.ball.pos.x);
      env.ball.pos.x=goalX+epsilonOut;
      env.ball.pos.y=intersectionY;
    }
    env.ball.velocity.zero();
  }
  return false;
}

// we check the center of the body of the player
bool R2Simulator::isPlayerOut(int player, int team){
  auto& pos= env.teams[team][player].pos;
  return ( (pos.x < pitch.x2) || (pos.x > pitch.x1) || (pos.y < pitch.y2) || (pos.y > pitch.y1) );
}

// we check the center of the body of the player
bool R2Simulator::isPlayerInsideHisArea(const int player, const int team){
  auto& pos= env.teams[team][player].pos;
  if( (team && (!env.halftimePassed) ) || ( (!team) && env.halftimePassed) )
    return ( (pos.x <= pitch.x1) && (pos.x >= pitch.areaRx) && (pos.y <= pitch.areaUy) && (pos.y >= pitch.areaDy) );
  else 
    return ( (pos.x >= pitch.x2) && (pos.x <= pitch.areaLx) && (pos.y <= pitch.areaUy) && (pos.y >= pitch.areaDy) );
}

// we check the center of the body of the player
bool R2Simulator::isPlayerInsideOpponentArea(const int player, const int team){
  auto& pos= env.teams[team][player].pos;
  if(! ( (team && (!env.halftimePassed) ) || ( (!team) && env.halftimePassed) ) )
    return ( (pos.x <= pitch.x1) && (pos.x >= pitch.areaRx) && (pos.y <= pitch.areaUy) && (pos.y >= pitch.areaDy) );
  else 
    return ( (pos.x >= pitch.x2) && (pos.x <= pitch.areaLx) && (pos.y <= pitch.areaUy) && (pos.y >= pitch.areaDy) );
}

// we check the full diameter of the body of the player
// team == 1: the player is of teams[1] and is checked against teams[0] area (left)
bool R2Simulator::isPlayerInsideOpponentAreaFullBody(Vec2 pos, const int team){
  if(! ( (team && (!env.halftimePassed) ) || ( (!team) && env.halftimePassed) ) ){
    bool insideCenterX=(pos.x <= pitch.x1) && (pos.x >= pitch.areaRx);
    bool insideCenterY=(pos.y <= pitch.areaUy) && (pos.y >= pitch.areaDy);
    if( insideCenterX && insideCenterY )
      return true;
    if(insideCenterX)
      return (( (pos.y-sett.playerRadius) <= pitch.areaUy) && ( (pos.y+sett.playerRadius) >= pitch.areaDy)) ;
    if(insideCenterY)
      return (( (pos.x-sett.playerRadius) <= pitch.x1) && ( (pos.x+sett.playerRadius) >= pitch.areaRx));
    // distance from upper or lower area corner
    Vec2 vert(pitch.areaRx, pitch.areaDy);
    if(pos.y>0.0){
      vert.y=pitch.areaUy;
    }
    return (pos.dist(vert) < sett.playerRadius);
  }
  else {
    bool insideCenterX=(pos.x >= pitch.x2) && (pos.x <= pitch.areaLx);
    bool insideCenterY=(pos.y <= pitch.areaUy) && (pos.y >= pitch.areaDy);
    if( insideCenterX && insideCenterY )
      return true;
    if(insideCenterX)
      return (( (pos.y-sett.playerRadius) <= pitch.areaUy) && ( (pos.y+sett.playerRadius) >= pitch.areaDy));
    if(insideCenterY)
      return (( (pos.x+sett.playerRadius) >= pitch.x2) && ( (pos.x-sett.playerRadius) <= pitch.areaLx));
    // distance from upper or lower area corner
    Vec2 vert(pitch.areaLx, pitch.areaDy);
    if(pos.y>0.0){
      vert.y=pitch.areaUy;
    }
    return (pos.dist(vert) < sett.playerRadius);
  }
  return false;
}

bool R2Simulator::isPlayerInsideOpponentAreaFullBody(const int player, const int team){
  return isPlayerInsideOpponentAreaFullBody( env.teams[team][player].pos, team);
}

void R2Simulator::limitBallSpeed(){
  double absSpeed=env.ball.absVelocity();
  if(absSpeed>sett.maxBallSpeed){
    double ratio=sett.maxBallSpeed/absSpeed;
    env.ball.velocity*=ratio;
  }
}

void R2Simulator::limitPlayerSpeed(R2PlayerInfo& p){
  double absSpeed=p.absVelocity();
  if(absSpeed>sett.maxPlayerSpeed)
  {
    double ratio=sett.maxPlayerSpeed/absSpeed;
    p.velocity*=ratio;
  }
}

void R2Simulator::limitSpeed(){
  limitBallSpeed();
  for(int i=0; i<2; i++)
    for(auto& p : env.teams[i])
        limitPlayerSpeed(p);
}

void R2Simulator::decayPlayerSpeed(R2PlayerInfo& p){
  if((p.velocity.x==0.0)&&(p.velocity.y==0.0))
    return;

  //finding and fix 2D gymbal lock
  double velAngle= atan2(p.velocity.y, p.velocity.x);
  double velAngleBis=fixAnglePositive(velAngle);
  double playerDir=p.direction;
  double diff=playerDir-velAngleBis;
  if(diff>M_PI)
    playerDir-=2*M_PI;
  else if(diff<-M_PI)
    velAngleBis-=2*M_PI;

  double len=p.velocity.len();
  double newAngle= velAngleBis*(1.0 - sett.playerVelocityDirectionMix) + playerDir*sett.playerVelocityDirectionMix;
  Vec2 newVelocity(newAngle);
  p.velocity=newVelocity*len*sett.playerVelocityDecay;
}

void R2Simulator::decaySpeed(){
  env.ball.velocity *= sett.ballVelocityDecay;
  
  for(auto& p : env.teams[0])
    decayPlayerSpeed(p);
  for(auto& p : env.teams[1])
    decayPlayerSpeed(p);
}

void R2Simulator::setBallCatchedPosition(){
    auto& goalkeeper=env.teams[env.ballCatchedTeam2][0];
    double d=sett.playerRadius -sett.ballRadius -sett.afterCatchDistance;
    env.ball.pos.x=goalkeeper.pos.x+cos(goalkeeper.direction)*d;
    env.ball.pos.y=goalkeeper.pos.y+sin(goalkeeper.direction)*d;
    env.ball.velocity=goalkeeper.velocity;
}
void R2Simulator::setBallReleasedPosition(){
    auto& goalkeeper=env.teams[env.ballCatchedTeam2][0];
    double d=sett.playerRadius +sett.ballRadius +sett.afterCatchDistance;
    env.ball.pos.x=goalkeeper.pos.x+cos(goalkeeper.direction)*d;
    env.ball.pos.y=goalkeeper.pos.y+sin(goalkeeper.direction)*d;
    env.ball.velocity=goalkeeper.velocity;
}

// team is the team to be put far from ball
void R2Simulator::putPlayersFarFromBall(int team, double minDist){
  for(auto& p: env.teams[team])
  {
    auto [dist, d]=p.dist(env.ball); 
    if(dist < minDist){
      if(dist<R2Epsilon) {
        double angle=uniformDist(rng)*2*M_PI;
        double sx=cos(angle)*minDist;
        double sy=sin(angle)*minDist;
          p.pos.x-=sx;
          p.pos.y-=sy;
      }
      else {
        double ratio=minDist/dist;
        double diff=ratio-1.0;
        if(diff>0.0){
          p.pos.x-=d.x*diff;
          p.pos.y-=d.y*diff;
        }
      }
    }
  }
}

//returns a new position that avoids, if possible, to intersect the other players that already acted.
Vec2 R2Simulator::avoidOtherPlayersPosition(Vec2 pos, int team, int player){  
  bool collisions=true;
  int count=0;
  while(collisions && (count <MaxCollisionLoop)){
    collisions=false;
    for(int w=0; w<=1; w++)
      for(int n=0; n< env.teams[w].size(); n++){
        if((w==team)&&(n==player))
          continue;
        auto other=env.teams[w][n];
        if(other.acted){
          Vec2 delta=other.pos-pos;
          double dist=delta.len();
          if(dist<R2Epsilon) {
            collisions=true;
            double angle=uniformDist(rng)*2*M_PI;
            double sx=cos(angle)*sett.playerRadius*2;
            double sy=sin(angle)*sett.playerRadius*2;
            pos.x+=sx;
            pos.y+=sy;
          }
          else {
            double ratio=sett.playerRadius*2/dist;
            if(ratio>1.0){
              collisions=true;
              double toAdd= ratio-1.0;
              pos.x+=delta.x*toAdd;
              pos.y+=delta.y*toAdd;
            }
          }
        }
      }

    count ++;
  }
  return pos;
}

void R2Simulator::actionMove(const R2Action& action, int team, int player){
  auto& p= env.teams[team][player];
  Vec2 pos(action.data[0],action.data[1]);

  p.direction=fixAnglePositive(action.data[2]);
  p.velocity.zero();

  //stay away from other people if they already moved
  pos=avoidOtherPlayersPosition(pos, team, player);
  p.pos=pos;
}

void R2Simulator::actionMoveKickoff(const R2Action& action, int team, int player){
  auto& p= env.teams[team][player];
  Vec2 pos(action.data[0],action.data[1]);

  p.direction=fixAnglePositive(action.data[2]);
  p.velocity.zero();

  //stay in your half pitch
  if(! ( (team && (!env.halftimePassed) ) || ( (!team) && env.halftimePassed) ) ){
    if (pos.x > -sett.playerRadius)
        pos.x=-sett.playerRadius;
  }
  else{
    if (pos.x < sett.playerRadius)
        pos.x=sett.playerRadius;
  }

  //stay far from the center circle
  double dueDistance=sett.centerRadius+sett.playerRadius;
  if( (env.state==R2State::Kickoff1) && team ){
      double dist=pos.len();
      if(dist<=R2Epsilon){
        pos.x=dueDistance;
        pos.y=uniformDist(rng);
      }
      else if (dist < dueDistance)
      {
        double ratio=dueDistance/dist;
        pos.x*=ratio;
        pos.y*=ratio;
      }
  }
  else if( (env.state==R2State::Kickoff2) && (!team) ){
      double dist=pos.len();
      if(dist<=R2Epsilon){
        pos.x=-dueDistance;
        pos.y=uniformDist(rng);
      }
      else if (dist < dueDistance)
      {
        double ratio=dueDistance/dist;
        pos.x*=ratio;
        pos.y*=ratio;
      }
  }

  //stay away from other people if they already moved
  pos=avoidOtherPlayersPosition(pos, team, player);
  p.pos=pos;;
}

void R2Simulator::actionMoveGoalkick(const R2Action& action, int team, int player){
  auto& p= env.teams[team][player];
  Vec2 pos(action.data[0],action.data[1]);
  double dir=fixAnglePositive(action.data[2]);

  Vec2 displace=pos-p.pos;
  //player of the kicking team: taking note of the longest movement
  if( ( ((env.state==R2State::Goalkick1up)||(env.state==R2State::Goalkick1down)) && (!team) ) || 
      ( ((env.state==R2State::Goalkick2up)||(env.state==R2State::Goalkick2down)) && team ) ){
    double movement=displace.len();
    if(movement>env.startingTeamMaxRange)
      env.startingTeamMaxRange=movement;
  }
  else { // check the max movement you can do
    if(displace.len()>env.startingTeamMaxRange){
      displace.resize(env.startingTeamMaxRange);
      pos=p.pos+displace;
    }
  }

  // if not of kicker team stay away fom the area
  if( ((env.state==R2State::Goalkick1up)||(env.state==R2State::Goalkick1down)) && team ){  // stay away from teams[0] area (left area)
    if(isPlayerInsideOpponentAreaFullBody(pos, true))
        pos.x=pitch.areaLx+sett.playerRadius;
  }
  else if( ((env.state==R2State::Goalkick2up)||(env.state==R2State::Goalkick2down)) && (!team) ) { // stay away from teams[1] area (right area)
    if(isPlayerInsideOpponentAreaFullBody(pos, false))
        pos.x=pitch.areaRx-sett.playerRadius;
  }

  //stay away from other people if they already moved
  pos=avoidOtherPlayersPosition(pos, team, player);

  p.velocity.zero();
  p.pos=pos;
  p.direction=dir; 
}

void R2Simulator::actionMoveThrowinCorner(const R2Action& action, int team, int player, double distanceToBall){
  auto& p= env.teams[team][player];
  Vec2 pos(action.data[0],action.data[1]);
  double dir=fixAnglePositive(action.data[2]);

  Vec2 displace=pos-p.pos;
  //player of the kicking team: taking note of the longest movement
  if( ( ((env.state==R2State::Throwin1)||(env.state==R2State::Corner1up)||(env.state==R2State::Corner1down)) && (!team) ) ||
      ( ((env.state==R2State::Throwin2)||(env.state==R2State::Corner2up)||(env.state==R2State::Corner2down)) && team ) ){
    double movement=displace.len();
    if(movement>env.startingTeamMaxRange)
      env.startingTeamMaxRange=movement;
  }
  else { // check the max movement you can do
    if(displace.len()>env.startingTeamMaxRange){
      displace.resize(env.startingTeamMaxRange);
      pos=p.pos+displace;
    }
  }

  // if not of kicker team stay away fom the ball
  if( ( ((env.state==R2State::Throwin1)||(env.state==R2State::Corner1up)||(env.state==R2State::Corner1down)) && team ) || 
      ( ((env.state==R2State::Throwin2)||(env.state==R2State::Corner2up)||(env.state==R2State::Corner2down)) && (!team) ) ){ 
    double d=pos.dist(env.ball.pos);
    while(d<distanceToBall){
      Vec2 line=pos-env.ball.pos;
      line.resize(distanceToBall);
      pos=env.ball.pos+line;
    }
  }

  //stay away from other people if they already moved
  pos=avoidOtherPlayersPosition(pos, team, player);

  p.velocity.zero();
  p.pos=pos;
  p.direction=dir; 
}

void R2Simulator::actionMoveThrowin(const R2Action& action, int team, int player){
  actionMoveThrowinCorner(action, team, player, sett.throwinMinDistance);
}

void R2Simulator::actionMoveCorner(const R2Action& action, int team, int player){
  actionMoveThrowinCorner(action, team, player, sett.cornerMinDistance);
}

void R2Simulator::actionDash(const R2Action& action, int team, int player){
  auto& p= env.teams[team][player];
  // limit power
  double power=action.data[1];
  double reverse=0.0;
  if(power < 0.0){
    power= -power;
    reverse=1.0;
  }
  power+=sett.playerRandomNoise*(normalDist(rng)-0.5)*action.data[1];  //add random
  if(power <0.0)
    power=0.0;

  if(power >=MaxDashPower)
    power=MaxDashPower;
  
  double angle=action.data[0] + reverse*M_PI + sett.playerDirectionNoise*(normalDist(rng)-0.5);

  p.velocity.x+=cos(angle)*power;
  p.velocity.y+=sin(angle)*power;
  p.direction=fixAnglePositive(angle);
}

void R2Simulator::actionKick(const R2Action& action, int team, int player){
  auto& p= env.teams[team][player];
  bool canKick=true;
  bool catchedKicking=false;

  if(env.ballCatched){
    if ((player!=0) || ( int(env.ballCatchedTeam2) != team)){
      canKick=false;
    }
    else{
      catchedKicking=true;
      env.ballCatched=0;
    }
  } 

  auto [dist, d]=p.dist(env.ball); 
  // is ball reachable ?
  if(dist>sett.kickableDistance)
    canKick=false;

  if(! sett.simplified){
    // is ball in front of player?
    if(!catchedKicking)
      if(canKick && (!isAnyTeamKicking())){
        double cosinusPlayerBall= (d.x*cos(p.direction) + d.y*sin(p.direction))/dist;
        if(cosinusPlayerBall < cosKickableAngle)
          canKick=false;
      }
  
    // is ball inside some player?
    if(!catchedKicking)
      if(canKick)
        for(int w=0; w<=1; w++)
          for(int n=0; n< env.teams[w].size(); n++){
            auto pl=env.teams[w][n];
            double d=(pl.pos-env.ball.pos).len();
            if(d<sett.playerRadius)  // well inside the player
              canKick=false;
          }
  }
  
  
  // limit power
  double reverse=0.0;
  double power=action.data[1];
  if(power < 0.0){
    power= -power;
    reverse=1.0;
  }
  power+=sett.playerRandomNoise*(normalDist(rng)-0.5)*action.data[1];  //add random
  if(power <0.0)
    power=0.0;

  double angle=action.data[0] + reverse*M_PI;
  double kickAngle=fixAnglePositive(angle);
  if(! sett.simplified){
    if(canKick && (!isAnyTeamKicking())){
      if( fabs( remainder( kickAngle-p.direction , 2*M_PI ) ) > sett.kickableDirectionAngle) // if angle between player direction and kick direction > kickableDirectionAngle or < -kickableDirectionAngle
        canKick=false;
    }
  }


  if(canKick && ballAlreadyKicked){
    if(! sett.simplified){
      if(uniformDist(rng)>ContemporaryKickProbability)
        canKick=false;
    }
    else{
      canKick=false;
    }
  }
  
  kickAngle+=sett.playerDirectionNoise*(normalDist(rng)-0.5);
  kickAngle=fixAnglePositive(kickAngle);

  p.direction=kickAngle;

  if(catchedKicking){ //move the ball to the first point outside the player
      setBallReleasedPosition();
  }
  else if(sett.simplified && canKick && (!isAnyTeamKicking()) ){ //if ball behind or too lateral, put it in front
    auto [dist, delta]=p.dist(env.ball); 
    double cosinusPlayerBall= (delta.x*cos(p.direction) + delta.y*sin(p.direction))/dist;
    if(cosinusPlayerBall < 0.707){ // less than 45 degrees
      double d=sett.playerRadius +sett.ballRadius +sett.afterCatchDistance;
      //std::cout << "old ball position: x=" << env.ball.pos.x << " y=" << env.ball.pos.y << "\n";
      env.ball.pos.x=p.pos.x+cos(kickAngle)*d;
      env.ball.pos.y=p.pos.y+sin(kickAngle)*d;
      //std::cout << "new ball position: x=" << env.ball.pos.x << " y=" << env.ball.pos.y << "\n";
      env.ball.velocity.x=0;
      env.ball.velocity.y=0;
    }
  }

  if(canKick){
    
    env.lastTouchedTeam2=team;
    if(power >= MaxKickPower)
      power=MaxKickPower;

    Vec2 kickDirection( kickAngle );

    env.ball.velocity.x = 0.0;
    env.ball.velocity.y = 0.0;

    env.ball.velocity += kickDirection*power;
    ballAlreadyKicked=true;
    
  }
}

void R2Simulator::actionCatch(const R2Action& action, int team, int player){
  if( (player >0) || (!isPlayerInsideHisArea(player, team)) )// only goalkeeper, inside his area
    return;

  if( (player>0)  || //only goalkeeper
      (!isPlayerInsideHisArea(player, team)) ) //inside his area
    return;

  if(env.ballCatched)
    return;

  // check if ball is in front and reachable
  auto& p= env.teams[team][player];

  auto [dist, d]=p.dist(env.ball); 
  // is ball reachable
  if(dist>(sett.catchableDistance) )
    return;

  // is ball in front of player?
  double cosinusPlayerBall= (d.x*cos(p.direction) + d.y*sin(p.direction))/dist;
  if(cosinusPlayerBall < cosCatchableAngle)  // angle > 90 or < -90 between player direction and ball direction
    return;

  // check catch probability
  if(uniformDist(rng) <=sett.catchProbability)
  {
    env.ball.velocity.x=0.0;
    env.ball.velocity.y=0.0;
    setBallCatchedPosition();
    env.lastTouchedTeam2=team;
    env.ballCatchedTeam2=team;
    env.ballCatched=sett.catchHoldingTicks;
  }
}

// is a team prepare for kicking from stopped game ?
bool R2Simulator::isAnyTeamPreparingKicking(){
  if(notStarterStates.count(env.state))
    return false;
  return true;
}

// is a team kicking from stopped game ?
bool R2Simulator::isAnyTeamKicking(){
  if((env.state==R2State::Play) && (notStarterStates.count(oldEnv.state)==0))
    return true;
  return false;
}

bool R2Simulator::isTeam2Kicking(R2State theState){
  if(team2StarterStates.count(theState))
    return true;
  return false;
}

void R2Simulator::resetPlayersActed(){
  //reset acting info
  for(int w=0; w<=1; w++)
    for(int n=0; n< env.teams[w].size(); n++){
      env.teams[w][n].acted=false;
    }
}

void R2Simulator::playersAct(){
  if(isAnyTeamPreparingKicking() ){ // preparing kicking: the kicking team acts first, with the closest player acting first.
    int kickingTeam= int(isTeam2Kicking(env.state));
    int sizeKickingTeam=teams[kickingTeam].size();
    // let's find the closest to the ball
    int closest=0;
    if(sizeKickingTeam){
      double minDist=env.teams[kickingTeam][0].pos.dist(env.ball.pos);
      for(int n=1; n< env.teams[kickingTeam].size(); n++){
        double distance=env.teams[kickingTeam][n].pos.dist(env.ball.pos);
        if(distance < minDist){
          minDist=distance;
          closest=n;
        }
      }

      //let's have the closest player acting first
      R2Action action = teams[kickingTeam][closest]->step( getGameState() ); 
      env.teams[kickingTeam][closest].acted=true;
      processStep(action, kickingTeam, closest);

      //then all of his own team except him
      for(int n=0; n< sizeKickingTeam; n++){
        if(n!=closest){
          action = teams[kickingTeam][n]->step( getGameState() ); // updated game state for each player
          env.teams[kickingTeam][n].acted=true;
          processStep(action, kickingTeam, n);
        }
      }
    }

    //then all other team
    int team= 1-kickingTeam;
    for(int n=0; n< env.teams[team].size(); n++){
      R2Action action = teams[team][n]->step( getGameState() ); // updated game state for each player
      env.teams[team][n].acted=true;
      processStep(action, team, n);
    }
  }
  else if(isAnyTeamKicking() ){ //kicking right now - only the kicking team's player that's closest to the ball starts first
    int kickingTeam= int(isTeam2Kicking(oldEnv.state));

    // let's find the closest to the ball
    int closest=0;
    if(teams[kickingTeam].size()){
      double minDist=env.teams[kickingTeam][0].pos.dist(env.ball.pos);
      for(int n=1; n< env.teams[kickingTeam].size(); n++){
        double distance=env.teams[kickingTeam][n].pos.dist(env.ball.pos);
        if(distance < minDist){
          minDist=distance;
          closest=n;
        }
      }

      //let's have the closest player of the kicking team acting first
      R2Action action = teams[kickingTeam][closest]->step( getGameState() );
      env.teams[kickingTeam][closest].acted=true;
      processStep(action, kickingTeam, closest);
    }

    //now all the rest, shuffled
    shuffle(begin(shuffledPlayers), end(shuffledPlayers), rng);
    for(int i: shuffledPlayers){
      int whichTeam=0;
      
      if(int index_team2= i - teams[0].size(); index_team2>=0){
          whichTeam = 1;
        i=index_team2;
      }
      if((i!=closest)||(whichTeam !=kickingTeam)){
        R2Action action = teams[whichTeam][i]->step( getGameState() ); // updated game state for each player
        env.teams[whichTeam][i].acted=true;
        processStep(action, whichTeam, i);
      }
    }
  }
  else{  // if not right after a stop-game begin, the player order is shuffled
    shuffle(begin(shuffledPlayers), end(shuffledPlayers), rng);
    auto gameState = getGameState();
    for(int i: shuffledPlayers){
      int whichTeam = 0;
      
      if(int index_team2= i - teams[0].size(); index_team2>=0){
          whichTeam = 1;
        i=index_team2;
      }
      R2Action action = teams[whichTeam][i]->step( gameState ); // same game state for each player
      env.teams[whichTeam][i].acted=true;
      processStep(action, whichTeam, i);
    }
  }
}

void R2Simulator::step(){
  history.envs[env.tick]=env;
  processedActions=0;

  resetPlayersActed();
  preState();
  playersAct();
  limitSpeed();
  limitPlayersCloseToPitch();
  checkState();
  decaySpeed();
  env.tick += 1;
};

bool R2Simulator::checkBallOut(){

  auto doBallLeftUp=[&](){
    if(env.lastTouchedTeam2){
      if(!env.halftimePassed)
        env.state=R2State::Goalkick1up;
      else
        env.state=R2State::Corner1up;
    }
    else{
      if(!env.halftimePassed)
        env.state=R2State::Corner2up;
      else
        env.state=R2State::Goalkick2up;      
    }
  };

  auto doBallLeftDown=[&](){
    if(env.lastTouchedTeam2){
      if(!env.halftimePassed)
        env.state=R2State::Goalkick1down;
      else
        env.state=R2State::Corner1down;
    }
    else{
      if(!env.halftimePassed)
        env.state=R2State::Corner2down;
      else
        env.state=R2State::Goalkick2down;
    }
  };

  auto doBallRightUp=[&](){
    if(env.lastTouchedTeam2){
      if(!env.halftimePassed)
        env.state=R2State::Corner1up;
      else 
        env.state=R2State::Goalkick1up;
    }
    else{
      if(!env.halftimePassed)
        env.state=R2State::Goalkick2up;
      else
        env.state=R2State::Corner2up;
    }
  };

  auto doBallRightDown=[&](){
    if(env.lastTouchedTeam2){
      if(!env.halftimePassed)
        env.state=R2State::Corner1down;
      else
        env.state=R2State::Goalkick1down;
    }
    else{
      if(!env.halftimePassed)
        env.state=R2State::Goalkick2down;
      else
        env.state=R2State::Corner2down;
    }
  };

  auto doBallLeft=[&](){
    if (env.ball.pos.y>0.0)
      doBallLeftUp();
    else
      doBallLeftDown();
  };

  auto doBallRight=[&](){
    if (env.ball.pos.y>0.0)
      doBallRightUp();
    else
      doBallRightDown();
  };

  Vec2 d=env.ball.pos - oldEnv.ball.pos;
  if(d.y==0.0){
    if(env.ball.pos.x < pitch.x2){
      doBallLeft();
      return true;
    }
    else if(env.ball.pos.x > pitch.x1){
      doBallRight();
      return true;
    }
    return false;
  }
  double ratio=d.x/d.y;

  if(isBallOutUp()){
    double du=pitch.y1-oldEnv.ball.pos.y;
    double hx=ratio*du+oldEnv.ball.pos.x;

    auto doBallUp=[&](){
      if(env.lastTouchedTeam2)
        env.state=R2State::Throwin1;
      else
        env.state=R2State::Throwin2;
    };

    if(isBallOutLeft()){
      if(hx<pitch.x2){ 
        doBallLeftUp();
      }
      else{ 
        doBallUp();
      }
    }
    else if(isBallOutRight()){
      if(hx>pitch.x1){  
        doBallRightUp();
      }
      else{ 
        doBallUp();
      }
    }
    else{ 
      doBallUp();
    }
    return true;
  }
  else if(isBallOutDown()){
    double du=pitch.y2-oldEnv.ball.pos.y;
    double hx=ratio*du+oldEnv.ball.pos.x;

    auto doBallDown=[&](){
      if(env.lastTouchedTeam2)
        env.state=R2State::Throwin1;
      else 
        env.state=R2State::Throwin2;
    };
    
    if(isBallOutLeft()){
      if(hx<pitch.x2){  
        doBallLeftDown();
      }
      else{ 
        doBallDown();
      }
    }
    else if(isBallOutRight()){
      if(hx>pitch.x1){  
        doBallRightDown();
      }
      else{ 
        doBallDown();
      }
    }
    else{ 
      doBallDown();
    }
    return true;
  }
  else if(isBallOutLeft()){
    doBallLeft();
    return true;
  }
  else if(isBallOutRight()){
    doBallRight();
    return true;
  }
  return false;
}

/**
@param s1 and s2 the initial and final points of the segment
@param c1 the center of the circle
@param r the radius of the circle
*/
// returns:
// int: number of intersections (0, 1, 2)
// double: t for first intersection (if any)
// double: t for second intersection (if existent)
///////////////////////////////////////////
// how it works:
// circle: (x - c1.x)^2 + (y - c1.y)^2 = r^2
// segment: x(t)= (s1.x - s2.x)*t + s1.x
//          y(t)= (s1.y - s2.y)*t + s1.y
// with 0 <= t <= 1
// putting together as in a system:
//  ((s1.x - s2.x)*t + s1.x - c1.x)^2 + ( (s1.y - s2.y)*t + s1.y - c1.y)^2 = r^2
// solving for t as a quadratic equation a*t^2 + b*t + c = 0
// with:
// a= (s1.x - s2.x)^2 - (s1.y - s2.y)^2
// b= 2*(s1.x - s2.x)*(s1.x - c.x) + 2*(s1.y - s2.y)*(s1.y - 1.y)
// c= (s1.x - c1.x)^2 + (s1.y - c1.y)^2 - r^2
// results:
// delta= b^2 - 4*a*c  // if <0 no intersection, if ==0 one tangent point, if >0 two intersections
// t= (b +- sqrt(delta)) / (2*a)
std::tuple<int, double, double> intersectionSegmentCircle(Vec2 s1, Vec2 s2, Vec2 c1, double r){
  Vec2 d= s1 - s2;
  Vec2 l= s1 - c1;
  double a= d.x*d.x - d.y*d.y;
  double b= 2.0*d.x*l.x + 2*d.y*l.y;
  double c= l.x*l.x + l.y*l.y - r*r;
  double delta= b*b - 4.0*a*c;
  if(delta < 0.0){
    return std::tuple<int, double, double>  { 0, 0.0, 0.0 };
  }
  else if(delta == 0.0){
    return std::tuple<int, double, double>  { 1, b/(2.0*a), 0.0 };
  }
  else{
    double deltaRoot= sqrt(delta);
    return std::tuple<int, double, double>  { 2, (b-deltaRoot)/(2.0*a), (b+deltaRoot)/(2.0*a) };
  }
}

// The ball is a circle moving by a rectilinear uniform motion during the tick (acceleration/deceleration changes the velocity only between a tick and the next) and it may be
// intersecting the player that is another circle moving by a rectilinear uniform motion.
// From a geometrical point of view, if considering a reference frame with respect to a circle (e.g. wrt the player), hence considering that reference circle as still,
// this is the same as having the other circle (e.g. the ball) moving by a rectilinear uniform motion whose uniform velocity is the resultant of the velocity of the moving circle minus
// the velocity of the "now-still" circle. 
// That in turn is equivalent to the intersection of a point moving by the same rectilinear trajectory (hence a line) intersecting a still circle that has the radius equal to 
// the sum of the radiuses of the two circles.
// THIS MEANS THAT WE NEED THE POSITION OF ALL PLAYER AND BALL OF THE PAST TICK.
// In this way if there is an intersection, the position of intersection will determine
// the position of the center of the ball when it collides with the player (before entering "inside" the player).
// (there may be zero intersections, or one if the line is tangent to the circle, or two if proper intersection).
// With zero intersections there is nothing to do.
// With one intersection I suggest to do nothing, there is not really an impact (one may want to calculate some friction effect though)
// With two intersection we need to find the closer in time.
// To transform this in the world where both circles are moving, its enough to consider the proportion of the trajectory line on which the intersection happened,
// then that is the proportion in which the uniform motion of the player and the ball collided.
// To simulate rightly what happens with all the players we should calculate the collision points of the ball with all the players, then
// take the collision that happened earlier: that is the only collision that actually happened.
// From that, calculate a new velocity/trajectory for the ball, considering only the remaining proportion of tick.
// Do the same, considering all the possible intersections (avoiding last intersected player) and go on until there is not an intersection anymore or until
// the maximum number of collision has ended.
// returns:
// bool: if intersection happened
// double: t of intersection
std::tuple<bool, double> R2Simulator::findObjectsCollision(R2ObjectInfo& obj1, R2ObjectInfo&obj2, double radius, double partialT)
{
  Vec2 s2= obj1.pos + (obj1.velocity - obj2.velocity)*(1.0 - partialT);
  auto [n, t1, t2]=intersectionSegmentCircle(obj1.pos, s2, obj2.pos, radius);
  if(n==2){ // if tangent, no collision really happened
    double t= (t1>=0.0) ? t1 : t2;  // we want the first intersection, unless it is less than 0.0 (that means there is not an actual intersection) in which case we check the second one(that is always greater than the first)
    if( (t>=R2Epsilon) && (t<=1.0) )
      return std::tuple<bool, double> {true, t} ;
  }
  return std::tuple<bool, double> {false, 0.0} ;
}

// returns:
// bool: if intersection happened
// double: t of intersection
std::tuple<bool, double> R2Simulator::findBallPlayerCollision(int team, int player, double partialT)
{
  return findObjectsCollision(env.ball, env.teams[team][player], sett.playerRadius+sett.ballRadius, partialT);
}

std::tuple<bool, double> R2Simulator::findPlayerPlayerCollision(int team1, int player1, int team2, int player2, double partialT){
    return findObjectsCollision(env.teams[team1][player1], env.teams[team2][player2], sett.playerRadius+sett.playerRadius, partialT);
}

std::tuple<bool, double> R2Simulator::findPoleObjectCollision(R2ObjectInfo& obj1, Vec2 pole, double radius, double partialT)
{
  Vec2 s2= obj1.pos + obj1.velocity*(1.0 - partialT);
  auto [n, t1, t2]=intersectionSegmentCircle(obj1.pos, s2, pole, radius);
  if(n==2){ // if tangent, no collision really happened
    double t= (t1>=0.0) ? t1 : t2;  // we want the first intersection, unless it is less than 0.0 (that means there is not an actual intersection) in which case we check the second one(that is always greater than the first)
    if( (t>=R2Epsilon) && (t<=1.0) )
      return std::tuple<bool, double> {true, t} ;
  }
  return std::tuple<bool, double> {false, 0.0} ;
}

// returns:
// bool: if intersection happened
// double: t of intersection
std::tuple<bool, double> R2Simulator::findPolePlayerCollision(int team, int player, Vec2 pole, double partialT){
  return findPoleObjectCollision(env.teams[team][player], pole, sett.playerRadius+sett.poleRadius, partialT);
}

std::tuple<bool, double> R2Simulator::findPoleBallCollision(Vec2 pole, double partialT){
  return findPoleObjectCollision(env.ball, pole, sett.ballRadius+sett.poleRadius, partialT);
}

R2PoleBallCollision R2Simulator::findFirstPoleBallCollision(double partialT){
  R2PoleBallCollision collision(false, 1.1, 0);
  for(int i=0; i<4; i++){
    auto[found, t]= findPoleBallCollision(pitch.poles[i], partialT);
    if(found) { // no need to do  && (t<collision.t) - only one collision possible
      collision.collision=true;
      collision.pole=i;
      collision.t=t;
      break; // can collide against only a pole at once
    }
  }
  return collision;
}

std::vector<R2PolePlayerCollision> R2Simulator::findFirstPolePlayersCollisions(double partialT){
  std::vector<R2PolePlayerCollision> collisions;
  double earlierT=1.1;
  for(int w=0; w<=1; w++)
    for(int n=0; n< env.teams[w].size(); n++){
      for(int i=0; i<4; i++){
        auto[found, t]= findPolePlayerCollision(w, n, pitch.poles[i], partialT);
        if(found){ 
          if(t<earlierT){
            collisions.clear();
          }
          if(t<=earlierT){
            earlierT=t;
            R2PolePlayerCollision collision(t, n, w, i);
            collisions.push_back(collision);
          }
          break;  // can collide against only a pole at once
        }
      }
    }
  return collisions;
}

std::vector<R2BallPlayerCollision> R2Simulator::findFirstBallPlayersCollisions(double partialT, std::vector<bool>& ballPlayerBlacklist){
  std::vector<R2BallPlayerCollision>  collisions;
  double earlierT=1.1;
  int t1size=env.teams[0].size();
  for(int w=0; w<=1; w++)
    for(int n=0; n< env.teams[w].size(); n++){
      if(ballPlayerBlacklist[t1size*w+n])
        continue;
      auto[found, t]= findBallPlayerCollision(w, n, partialT);
      if(found){ 
        if(t<earlierT){
          collisions.clear();
        }
        if(t<=earlierT){
          earlierT=t;
          R2BallPlayerCollision collision(t, n, w);
          collisions.push_back(collision);
        }
      }

    }
  return collisions;
}

std::vector<R2PlayerPlayerCollision> R2Simulator::findFirstPlayerPlayersCollisions(double partialT, std::vector<int>& playerPlayerCollisions){
  std::vector<R2PlayerPlayerCollision> collisions;
  int t1size=env.teams[0].size();
  double earlierT=1.1;
  for(int w1=0; w1<=1; w1++){
    int l1= env.teams[w1].size();
    for(int w2=0; w2<=1; w2++){
      if( w1 && (!w2) ) // do not check twice the collision betwenn the two teams
        continue;
      int l2= env.teams[w2].size();
        for(int n1=0; n1<l1; n1++){
          int start2 = (w1 == w2) ? (n1+1) : 0 ;
          for(int n2=start2; n2<l2; n2++){
              if(playerPlayerCollisions[w1*t1size+n1]!= (w2*t1size+n2) ){ // only if not just prior collision
                auto[found, t]= findPlayerPlayerCollision(w1, n1, w2, n2, partialT);
                if(found){ 
                  if(t<earlierT){
                    collisions.clear();
                  }
                  if(t<=earlierT){
                    earlierT=t;
                    R2PlayerPlayerCollision collision(t, n1, w1, n2, w2);
                    collisions.push_back(collision);
                  }
                }
              }
          }
        }
    }
  }
  return collisions;
}

// updates motion up to t
void R2Simulator::updateMotion(double t){
  env.ball.pos+=env.ball.velocity*t;
  for(int w=0; w<=1; w++)
    for(auto& p: env.teams[w]){
      p.pos+=p.velocity*t;
    }
} 

void R2Simulator::addBallNoise(){
  Vec2 noise=  Vec2((normalDist(rng)-0.5)*fabs(env.ball.velocity.x), (normalDist(rng)-0.5)*fabs(env.ball.velocity.y))*sett.playerRandomNoise;
  Vec2 newPos= env.ball.pos + noise;

  for(int w=0; w<=1; w++)
    for(int n=0; n< env.teams[w].size(); n++){
      auto& p= env.teams[w][n];
      if( (newPos.x<=(p.pos.x+sett.playerRadius+sett.ballRadius)) &&
          (newPos.x>=(p.pos.x-sett.playerRadius-sett.ballRadius)) &&
          (newPos.y<=(p.pos.y+sett.playerRadius+sett.ballRadius)) &&
          (newPos.y>=(p.pos.y+sett.playerRadius+sett.ballRadius)) ){
        return;
      }
    }
  
  env.ball.pos=newPos;
}

bool R2Simulator::checkGoalOrBallOut(){
  if (isGoalScored(false)) {
          env.score2 += 1;
          env.state = R2State::Goal2;
          env.ball.velocity.zero();
          return true;
  }
  else if (isGoalScored(true)) {
          env.score1 += 1;
          env.state = R2State::Goal1;
          env.ball.velocity.zero();
          return true;
  }
  
  return checkBallOut();
}

void R2Simulator::manageCollisions(){
  double partialT= 0.0;
  bool collisions=true;
  int count=0;
  double addT=0.0;

  int t1size=env.teams[0].size();
  int t2size=env.teams[1].size();

  std::vector<int> playerPlayerCollisions(t1size+t2size, -1);
  std::vector<bool>  ballPlayerBlacklist(t1size+t2size, false);

  int ballPlayersColls[MaxCollisionInsideTickLoop]; 
  int ballPlayersCollsTeam[MaxCollisionInsideTickLoop]; 
  int howManyBallPlayersColls=0;
  while(collisions && (count <MaxCollisionInsideTickLoop) && (partialT<1.0) ){
    collisions=false;
    std::vector<R2CollisionTime> earlierCollisionsTypes;

    R2PoleBallCollision newPoleBallColl(false, 0.0, 0);
    if(!env.ballCatched){
      newPoleBallColl=findFirstPoleBallCollision(partialT);
      if((newPoleBallColl.collision)&&(env.ball.velocity.len()>0.0)){
        collisions|=newPoleBallColl.collision;

        if(earlierCollisionsTypes.size()>0){
          double earlierT=earlierCollisionsTypes[0].t;
          if(newPoleBallColl.t<earlierT){
            earlierCollisionsTypes.clear();
          }
          if(newPoleBallColl.t<=earlierT){
            R2CollisionTime co(newPoleBallColl.t, R2CollisionType::PoleBall);
            earlierCollisionsTypes.push_back(co);
          }
        }
        else{
          R2CollisionTime co(newPoleBallColl.t, R2CollisionType::PoleBall);
          earlierCollisionsTypes.push_back(co);
        }
      }
    }

    std::vector<R2PolePlayerCollision> newPolePlayersColls= findFirstPolePlayersCollisions(partialT);
    bool collPolePlayers=(newPolePlayersColls.size()>0);
    collisions|=collPolePlayers;
    double kPolePlayers=0.0;
    if(collPolePlayers){
     kPolePlayers=newPolePlayersColls[0].t;
    }
    if(collPolePlayers){
      if(earlierCollisionsTypes.size()>0){
        double earlierT=earlierCollisionsTypes[0].t;
        if(kPolePlayers<earlierT){
          earlierCollisionsTypes.clear();
        }
        if(kPolePlayers<=earlierT){
          R2CollisionTime co(kPolePlayers, R2CollisionType::PolePlayer);
          earlierCollisionsTypes.push_back(co);
        }
      }
      else{
        R2CollisionTime co(kPolePlayers, R2CollisionType::PolePlayer);
        earlierCollisionsTypes.push_back(co);
      }
    }

    std::vector<R2BallPlayerCollision> newBallPlayerColls;
    if(!env.ballCatched){
      newBallPlayerColls=findFirstBallPlayersCollisions(partialT, ballPlayerBlacklist);
      bool collBall=(newBallPlayerColls.size()>0);
      double kBall=0.0;
      if(collBall){
        kBall=newBallPlayerColls[0].t;
      }
      collisions|=collBall;
      if(collBall){
        if(earlierCollisionsTypes.size()>0){
          double earlierT=earlierCollisionsTypes[0].t;
          if(kBall<earlierT){
            earlierCollisionsTypes.clear();
          }
          if(kBall<=earlierT){
            R2CollisionTime co(kBall, R2CollisionType::BallPlayer);
            earlierCollisionsTypes.push_back(co);
          }
        }
        else{
          R2CollisionTime co(kBall, R2CollisionType::BallPlayer);
          earlierCollisionsTypes.push_back(co);
        }
      }
    }

    std::vector<R2PlayerPlayerCollision> newPlayerPlayerColls=findFirstPlayerPlayersCollisions(partialT, playerPlayerCollisions);
    bool collPlayers=(newPlayerPlayerColls.size()>0);
    double kPlayers=0.0;
    if(collPlayers){
      kPlayers=newPlayerPlayerColls[0].t;
    }
    collisions|=collPlayers;

    if(collPlayers){
      if(earlierCollisionsTypes.size()>0){
        double earlierT=earlierCollisionsTypes[0].t;
        if(kPlayers<earlierT){
          earlierCollisionsTypes.clear();
        }
        if(kPlayers<=earlierT){
          R2CollisionTime co(kPlayers, R2CollisionType::PlayerPlayer);
          earlierCollisionsTypes.push_back(co);
        }
      }
      else{
        R2CollisionTime co(kPlayers, R2CollisionType::PlayerPlayer);
        earlierCollisionsTypes.push_back(co);
      }
    }

    if(collisions){
      double earlierT=earlierCollisionsTypes[0].t;
      addT=earlierT*(1.0 - partialT);
      updateMotion(addT);

      for(auto& co: earlierCollisionsTypes){
        if(co.type==R2CollisionType::BallPlayer){
          for(auto bpcoll:newBallPlayerColls){
            //avoiding continuous bouncing and ball entering the player
            ballPlayersColls[howManyBallPlayersColls]=bpcoll.p;
            ballPlayersCollsTeam[howManyBallPlayersColls]=bpcoll.team;
            howManyBallPlayersColls++;

            if(howManyBallPlayersColls>=4){
              if( (ballPlayersColls[howManyBallPlayersColls-1]==ballPlayersColls[howManyBallPlayersColls-3]) &&
                (ballPlayersColls[howManyBallPlayersColls-2]==ballPlayersColls[howManyBallPlayersColls-4]) &&
                (ballPlayersCollsTeam[howManyBallPlayersColls-1]==ballPlayersCollsTeam[howManyBallPlayersColls-3]) &&
                (ballPlayersCollsTeam[howManyBallPlayersColls-2]==ballPlayersCollsTeam[howManyBallPlayersColls-4])
                ){

                env.ball.velocity.zero();

                auto& p1= env.teams[int(ballPlayersCollsTeam[howManyBallPlayersColls-1])][ballPlayersColls[howManyBallPlayersColls-1]];
                p1.velocity.zero();
                auto& p2= env.teams[int(ballPlayersCollsTeam[howManyBallPlayersColls-2])][ballPlayersColls[howManyBallPlayersColls-2]];
                p2.velocity.zero();

                count++;
                continue;
              }
            }
            env.lastTouchedTeam2=bool(bpcoll.team);
            //let's change ball velocity
            auto& p= env.teams[bpcoll.team][bpcoll.p];
            Vec2 v=env.ball.velocity - p.velocity;
            double vel=v.len();
            Vec2 d=env.ball.pos - p. pos;
            if( ((v.x==0.0)&&(v.y==0.0)) || ((d.x==0.0)&&(d.y==0.0)) ){  // this happens if the ball is moving exactly at the same velocity as the player, they just intersected and floating numbers have some rounding errors
              // blacklist the player so it won't be checked continuously for the collision
              ballPlayerBlacklist[t1size*bpcoll.team+bpcoll.p]=true;
              count++;
              continue;
            }
            double impactAngle=atan2(d.y, d.x); // angle of the impact point on the player's circle wrt to player center
            double trajectoryAngle=atan2(v.y, v.x); // ball trajectory angle is the same as ball velocity direction, with inverted sign to have it on the same orientation of the d vector
            double reflectedAngle=impactAngle + remainder(impactAngle-trajectoryAngle, M_PI);
          
            double rX=cos(reflectedAngle);
            double rY=sin(reflectedAngle);
            
            if(vel > 0.0){
              if(! sett.simplified){
                if( fabs(remainder(p.direction-impactAngle, 2*M_PI)) < KickableAngle ){  // bounces on the back of a player, stopped in front of the player
                  env.ball.velocity.x=p.velocity.x*BallPlayerHitFactor +rX*vel*BallPlayerStopFactor;
                  env.ball.velocity.y=p.velocity.y*BallPlayerHitFactor +rY*vel*BallPlayerStopFactor;
                }
                else{
                  env.ball.velocity.x=p.velocity.x*BallPlayerHitFactor +rX*vel*BallPlayerBounceFactor;
                  env.ball.velocity.y=p.velocity.y*BallPlayerHitFactor +rY*vel*BallPlayerBounceFactor;
                }
              }
              else{ // simplified model
                if( fabs(remainder(p.direction-impactAngle, 2*M_PI)) < KickableAngle ){  // bounces on the back of a player, stopped in front of the player
                  env.ball.velocity.x=p.velocity.x*BallPlayerHitFactorSimplified;
                  env.ball.velocity.y=p.velocity.y*BallPlayerHitFactorSimplified;
                }
                else{
                  env.ball.velocity.x=p.velocity.x*BallPlayerHitFactorSimplified +rX*vel*BallPlayerBounceFactor;
                  env.ball.velocity.y=p.velocity.y*BallPlayerHitFactorSimplified +rY*vel*BallPlayerBounceFactor;
                }
              }
            }
          }
        }
        else if(co.type==R2CollisionType::PlayerPlayer){
          for(auto ppcoll:newPlayerPlayerColls){
            playerPlayerCollisions[ppcoll.team1*t1size+ppcoll.p1]=ppcoll.team2*t1size+ppcoll.p2;  // take note of collision
            playerPlayerCollisions[ppcoll.team2*t1size+ppcoll.p2]=ppcoll.team1*t1size+ppcoll.p1;

            //let's change players velocity
            auto& p1= env.teams[ppcoll.team1][ppcoll.p1];
            auto& p2= env.teams[ppcoll.team2][ppcoll.p2];
            // the mass is supposed equal for players.
            // modification to the first player
            Vec2 v1=p1.velocity - p2.velocity;  // velocity of p1 wrt p2 (just like p2 was still), that is total velocity of p1 impacting AGAINST p2
            Vec2 d1=p2.pos - p1.pos;
            double transmission=v1.cosBetween(d1) *0.5;  // the more the impact angle (depending on impact point, or the centers) coincides with the resulting relative velocity, the more the energy is transferred to the impact. It has to be divided by two (an half for each player)
            double momentum=v1.len()*transmission;  // the momentum depends on the resulting velocity magnitude, on the transmission
            // a part of inverted velocity goes to acceleration/deceleration depending on how much the direction coincides with the player direction
            Vec2 v2=v1*(-1);  // actual velocity vector of the impact AGAINST p1
            Vec2 dir1= Vec2(p1.direction) ;
            double accelPart1=v2.cosBetween(dir1);  // part of momentum to be used to accelerate/decelerate the player because aligned with its direction (the other part would displace a little the player)

            Vec2 dir2= Vec2(p2.direction) ;
            double accelPart2=v1.cosBetween(dir2);

            Vec2 accel1= dir1 * accelPart1 * momentum;
            Vec2 accel2= dir2 * accelPart2 * momentum;
            Vec2 displace1 = v2 * transmission -accel1 ; // ciò che rimane viene usato per l'accelerazione di tipo "displace" che è minore
            Vec2 displace2 = v1 * transmission -accel2 ;

            //Vec2 oldV1=p1.velocity;
            //Vec2 oldV2=p2.velocity;

            p1.velocity += accel1;
            p2.velocity += accel2;
            
            p1.velocity += displace1*CollisionPlayerDisplaceFactor;
            p2.velocity += displace2*CollisionPlayerDisplaceFactor;

            // let's stop completely the player if he crashed frontally
            /*
            // this was working decently
            if(accelPart1<0.0)
              p1.velocity.zero();
            if(accelPart2<0.0)
              p2.velocity.zero();
              */
            
            /*
            // this was working good enough
            if(d1.cosBetween(oldV1)<0.0)
              p1.velocity.zero();
            if(d1.cosBetween(oldV2)>0.0)
              p2.velocity.zero();
              */

            /*
            // this was working well and it's a little more principled and better working than the one above
            if(d1.cosBetween(p1.velocity)<0.0)
              p1.velocity.zero();
            if(d1.cosBetween(p2.velocity)>0.0)
              p2.velocity.zero();
              */

            // cancel only the frontal crash velocity component and not all the velocity: it results in a lesser blocking of players than the commented mechanisms above 
            d1.resize(1.0);
            Vec2 d2=d1*-1;
            double cosV1=d1.cosBetween(p1.velocity);
            if(cosV1>0.0){
              Vec2 toSub=d1*cosV1*p1.velocity.len();  // projection of p1 velocity onto the line connecting p1 and 2
              p1.velocity -= toSub;                   // subtract the crash direction component of the velocity
            }
            double cosV2=d2.cosBetween(p2.velocity);
            if(cosV2>0.0){
              Vec2 toSub=d2*cosV2*p2.velocity.len();  // projection of p2 velocity onto the line connecting p1 and 2
              p2.velocity -= toSub;                   // subtract the crash direction component of the velocity
            }
          }
        }
        else if(co.type==R2CollisionType::PoleBall){
          Vec2 pole=pitch.poles[newPoleBallColl.pole];

          double vel=env.ball.velocity.len();
          Vec2 d=env.ball.pos - pole;

          double impactAngle=atan2(d.y, d.x); // angle of the impact point on the pole circle wrt to pole center
          double trajectoryAngle=atan2(env.ball.velocity.y, env.ball.velocity.x); 
          double reflectedAngle=impactAngle + remainder(impactAngle-trajectoryAngle, M_PI);
          double rX=cos(reflectedAngle);
          double rY=sin(reflectedAngle);
          env.ball.velocity.x=rX*vel*BallPoleBounceFactor;
          env.ball.velocity.y=rY*vel*BallPoleBounceFactor;
          
        }
        else if(co.type==R2CollisionType::PolePlayer){
            for(auto ppcoll: newPolePlayersColls){
              auto& p1= env.teams[ppcoll.team][ppcoll.p];
              p1.velocity.zero();

              Vec2 pole=pitch.poles[ppcoll.pole];

              Vec2 d=(p1.pos-pole); // let's put just a little distance from the pole
              d.resize(sett.poleRadius+sett.playerRadius+R2Epsilon);
              p1.pos=pole+d;
            }

        }
      }

      partialT+=addT;
    }
    count ++;

    if(checkGoalOrBallOut()) {
      collisions=false;
    }
    else {
      oldEnv = env;
      manageBallInsidePlayers();
      if(checkGoalOrBallOut()) {
        collisions=false;
      }
      else{
        oldEnv = env;
      }
    }
  }

  if(env.ballCatched){
    setBallCatchedPosition();
  }

  //final series of checks
  if (env.state == R2State::Play) {
      updateMotion(1.0 - partialT); // the rest of the tick has to be completed
      addBallNoise();

      if (! checkGoalOrBallOut()) {
          oldEnv = env;
          manageBallInsidePlayers();
          if (! checkGoalOrBallOut()) {
            oldEnv = env;
          }
      } 
  }
}

void R2Simulator::manageBallInsidePlayers(){
  const double radius=sett.playerRadius+sett.ballRadius;
  if(env.ballCatched)
    return;
  bool collisions=true;

  auto checkCol=[&](int team){
    for(auto& p: env.teams[team])
    {
      Vec2 d=env.ball.pos - p.pos;

      double len=d.len();
      if((len+R2SmallEpsilon)< radius){
        collisions=true;

        d.resize(radius+R2SmallEpsilon);
        Vec2 oldPos=env.ball.pos;
        env.ball.pos=p.pos+d;
        Vec2 displace=env.ball.pos-oldPos;
        env.ball.velocity += displace*sett.ballInsidePlayerVelocityDisplace;

      }
    }
  };

  int count=0;
  while(collisions && (count <MaxCollisionLoop)){
    collisions=false;
    checkCol(0);
    checkCol(1);
    count ++;
  }
}

void R2Simulator::updateCollisionsAndMovements(){
  manageCollisions();
  manageStaticPlayersCollisions();
}

// here in case of collision the player is moved
void R2Simulator::manageStaticBallCollisions(){
  bool collisions=true;
  int count=0;
  while(collisions && (count <MaxCollisionLoop)){
    collisions=false;
    
    for(int team=0; team<=1; team++)
      for(auto& p: env.teams[team])
      {
        auto [dist, d]=p.dist(env.ball); 
        if(dist<R2Epsilon) {
          collisions=true;
          double angle=uniformDist(rng)*2*M_PI;
          double sx=cos(angle)*(sett.playerRadius+sett.ballRadius);
          double sy=sin(angle)*(sett.playerRadius+sett.ballRadius);
            p.pos.x-=sx;
            p.pos.y-=sy;
        }
        else {
          double ratio=(sett.playerRadius+sett.ballRadius)/dist;
          double diff=ratio-1.0;
          if(diff>0.0){
            collisions=true;
            p.pos.x+=d.x*diff;
            p.pos.y+=d.y*diff;
          }
        }
      }

    collisions|=manageStaticPoleBallCollisions();
    count ++;
  }
}

// player vs player collisions, when the game is inactive
void R2Simulator::manageStaticPlayersCollisions(){
  bool collisions=true;

  auto checkCol=[&](int t1, int t2){
    
    int c1=0;
    for(auto& p1: env.teams[t1]){
      c1++;
      int c2=0;
      for(auto& p2: env.teams[t2]){
        c2++;
        if(&p1 == &p2)
          continue;

        auto [dist, d]=p2.dist(p1); 

        if(dist<R2Epsilon) {
          collisions=true;
          double angle=uniformDist(rng)*2*M_PI;
          double sx=cos(angle)*sett.playerRadius;
          double sy=sin(angle)*sett.playerRadius;
            p1.pos.x+=sx;
            p1.pos.y+=sy;
            p2.pos.x-=sx;
            p2.pos.y-=sy;
        }
        else if(dist<sett.playerRadius*2){
            double ratio=sett.playerRadius*2/dist;
            collisions=true;
            double toAdd= ratio-1.0;
            
            p1.pos.x+=d.x*toAdd*0.5;
            p1.pos.y+=d.y*toAdd*0.5;
            p2.pos.x-=d.x*toAdd*0.5;
            p2.pos.y-=d.y*toAdd*0.5;

        }
        
      }
    }
  };

  int count=0;
  while(collisions && (count <MaxCollisionLoop)){
    collisions=false;
    checkCol(0, 1);
    checkCol(0, 0);
    checkCol(1, 1);
    collisions|=manageStaticPolePlayersCollisions();
    count ++;
  }
}

bool R2Simulator::manageStaticPoleBallCollisions(){
  const double radius=sett.poleRadius+sett.ballRadius;

  for(auto& pole: pitch.poles)
  {
    Vec2 d=env.ball.pos-pole; 
    double dist=d.len();
    if(dist<R2Epsilon) {
      double angle=uniformDist(rng)*2*M_PI;
      double sx=cos(angle)*radius;
      double sy=sin(angle)*radius;
        env.ball.pos.x-=sx;
        env.ball.pos.y-=sy;
        return true;
    }
    else {
      double ratio=radius/dist;
      double diff=ratio-1.0;
      if(diff>0.0){
        env.ball.pos.x+=d.x*diff;
        env.ball.pos.y+=d.y*diff;
        return true;
      }
    }
  }
  return false;
}

bool R2Simulator::manageStaticPolePlayersCollisions(){
  const double radius=sett.poleRadius+sett.playerRadius;
  bool collisions=true;

  int count=0;
  while(collisions && (count <MaxCollisionLoop)){
    collisions=false;
    
    for(int team=0; team<=1; team++)
      for(auto& p: env.teams[team])
      {
        for(auto& pole: pitch.poles)
        {
          Vec2 d=p.pos-pole; 
          double dist=d.len();
          if(dist<R2Epsilon) {
            collisions=true;
            double angle=uniformDist(rng)*2*M_PI;
            double sx=cos(angle)*radius;
            double sy=sin(angle)*radius;
              p.pos.x-=sx;
              p.pos.y-=sy;
          }
          else {
            double ratio=radius/dist;
            double diff=ratio-1.0;
            if(diff>0.0){
              collisions=true;
              p.pos.x+=d.x*diff;
              p.pos.y+=d.y*diff;
            }
          }
        }
      }

    count ++;
  }
  return collisions;
}

void R2Simulator::preState(){
  switch(env.state)
  {
    case R2State::Inactive: 
      break;
    case R2State::Ready:  // currently unused
      break;
    case R2State::Kickoff1:
      env.ballCatched=0;
      env.ball.pos.zero();
      env.ball.velocity.zero();
      break;
    case R2State::Kickoff2:
      env.ballCatched=0;
      env.ball.pos.zero();
      env.ball.velocity.zero();
      break;
    case R2State::Play:
      if(env.ballCatched){
        if(!isPlayerInsideHisArea(0, int(env.ballCatchedTeam2))){
          env.ballCatched=0;  //goalkeeper exited his area, balls drop
        }
        else{
          env.ballCatched--;
        }
        
        if(env.ballCatched==0){
          setBallReleasedPosition();  // position ball in front of goalkeeper
        }
      }
      ballAlreadyKicked=false;
      env.startingTeamMaxRange=0.0;
      break;
    case R2State::Stopped:
      env.ballCatched=0;
      setBallReleasedPosition();
      env.ball.velocity.zero();
      break;
    case R2State::Goalkick1up:
      env.ballCatched=0;
      if(!env.halftimePassed) 
        env.ball.pos.x=pitch.goalKickLx;
      else
        env.ball.pos.x=pitch.goalKickRx;
      env.ball.pos.y=pitch.goalKickUy;
      env.ball.velocity.zero();
      env.startingTeamMaxRange=0.0;
      break;
    case R2State::Goalkick1down:
      env.ballCatched=0;
      if(!env.halftimePassed) 
        env.ball.pos.x=pitch.goalKickLx;
      else
        env.ball.pos.x=pitch.goalKickRx; 
      env.ball.pos.y=pitch.goalKickDy;
      env.ball.velocity.zero();
      env.startingTeamMaxRange=0.0;
      break;
    case R2State::Goalkick2up:
      env.ballCatched=0;
      if(!env.halftimePassed) 
        env.ball.pos.x=pitch.goalKickRx;
      else
        env.ball.pos.x=pitch.goalKickLx;
      env.ball.pos.y=pitch.goalKickUy;
      env.ball.velocity.zero();
      env.startingTeamMaxRange=0.0;
      break;
    case R2State::Goalkick2down:
      env.ballCatched=0;
      if(!env.halftimePassed) 
        env.ball.pos.x=pitch.goalKickRx;
      else
        env.ball.pos.x=pitch.goalKickLx;
      env.ball.pos.y=pitch.goalKickDy;
      env.ball.velocity.zero();
      env.startingTeamMaxRange=0.0;
      break;
    case R2State::Corner1up:
      env.ballCatched=0;
      if(!env.halftimePassed) 
        env.ball.pos.x=pitch.x1;
      else
        env.ball.pos.x=pitch.x2;
      env.ball.pos.y=pitch.y1;
      env.ball.velocity.zero();
      env.startingTeamMaxRange=0.0;
      break;
    case R2State::Corner1down:
      env.ballCatched=0;
      if(!env.halftimePassed) 
        env.ball.pos.x=pitch.x1;
      else
        env.ball.pos.x=pitch.x2;
      env.ball.pos.y=pitch.y2;
      env.ball.velocity.zero();
      env.startingTeamMaxRange=0.0;
      break;
    case R2State::Corner2up:
      env.ballCatched=0;
      if(!env.halftimePassed) 
        env.ball.pos.x=pitch.x2;
      else
        env.ball.pos.x=pitch.x1;
      env.ball.pos.y=pitch.y1;
      env.ball.velocity.zero();
      env.startingTeamMaxRange=0.0;
      break;
    case R2State::Corner2down:
      env.ballCatched=0;
      if(!env.halftimePassed) 
        env.ball.pos.x=pitch.x2;
      else
        env.ball.pos.x=pitch.x1;
      env.ball.pos.y=pitch.y2;
      env.ball.velocity.zero();
      env.startingTeamMaxRange=0.0;
      break;
    case R2State::Throwin1:
      env.ballCatched=0;
      setBallThrowInPosition();
      env.ball.velocity.zero();
      env.startingTeamMaxRange=0.0;
      break;
    case R2State::Throwin2:
      env.ballCatched=0;
      setBallThrowInPosition();
      env.ball.velocity.zero();
      env.startingTeamMaxRange=0.0;
      break;
    case R2State::Paused:
      break;
    case R2State::Halftime:
      env.ballCatched=0;
      break;
    case R2State::Goal1:
      break;
    case R2State::Goal2:
      break;
    case R2State::Ended:
      break;
    default:
      break;
  } 
}

void R2Simulator::checkState(){
  switch(env.state)
  {
    case R2State::Inactive: 
      break;
    case R2State::Ready: 
      break;
    case R2State::Kickoff1:
      limitPlayersToHalfPitchFullBody(0);
      manageStaticBallCollisions();
      manageStaticPlayersCollisions();
      manageStaticBallCollisions();
      limitPlayersToHalfPitchFullBody(0);
      oldEnv = env;
      env.state=R2State::Play;
      break;
    case R2State::Kickoff2:
      limitPlayersToHalfPitchFullBody(1);
      manageStaticBallCollisions();
      manageStaticPlayersCollisions();
      manageStaticBallCollisions();
      limitPlayersToHalfPitchFullBody(1);
      oldEnv = env;
      env.state=R2State::Play;
      break;
    case R2State::Play:
      updateCollisionsAndMovements();
      break;
    case R2State::Stopped:
      break;
    case R2State::Goalkick1up:
    case R2State::Goalkick1down:
      limitPlayersOutsideAreaFullBody(false);
      manageStaticBallCollisions();
      manageStaticPlayersCollisions();
      limitPlayersOutsideAreaFullBody(false);
      oldEnv = env;
      env.state=R2State::Play;
      break;
    case R2State::Goalkick2up:
    case R2State::Goalkick2down:
      limitPlayersOutsideAreaFullBody(true);
      manageStaticBallCollisions();
      manageStaticPlayersCollisions();
      limitPlayersOutsideAreaFullBody(true);
      oldEnv = env;
      env.state=R2State::Play;
      break;
    case R2State::Corner1up:
    case R2State::Corner1down:
      putPlayersFarFromBall(1, sett.cornerMinDistance);
      oldEnv = env;
      env.state=R2State::Play;
      break;
    case R2State::Corner2up:
    case R2State::Corner2down:
      putPlayersFarFromBall(0, sett.cornerMinDistance);
      oldEnv = env;
      env.state=R2State::Play;
      break;
    case R2State::Throwin1:
      putPlayersFarFromBall(1, sett.throwinMinDistance);
      oldEnv = env;
      env.state=R2State::Play;
      break;
    case R2State::Throwin2:
      putPlayersFarFromBall(0, sett.throwinMinDistance);
      oldEnv = env;
      env.state=R2State::Play;
      break;

    case R2State::Paused:
      break;
    case R2State::Halftime:
      break;
    case R2State::Goal1:
      oldEnv = env;
      env.state=R2State::Kickoff2;
      break;
    case R2State::Goal2:
      oldEnv = env;
      env.state=R2State::Kickoff1;
      break;
    case R2State::Ended:
      break;
    default:
      break;
  } 
}

void R2Simulator::setStartMatch() {
  startedTeam2=false;
  if(uniformDist(rng) >= 0.5)
    startedTeam2=true;
  env.state= startedTeam2 ? R2State::Kickoff2 : R2State::Kickoff1;
}

void R2Simulator::setHalfTime() {
    env.state= (!startedTeam2) ? R2State::Kickoff2 : R2State::Kickoff1;
    env.halftimePassed= true;
}

/*
// this was working well too.
void R2Simulator::playMatch() {
  setStartMatch();
  for(int i=0; i<2; i++)
  {
    for(int t=0; t<sett.ticksPerTime; t++)
    {
        step();
    }
    setHalfTime();
  }
  history.envs.back()=env;  // log also final environment
}
*/

void R2Simulator::playMatch() {
  while(stepIfPlaying());
}

bool R2Simulator::stepIfPlaying(){
  if(env.tick==0)
	  setStartMatch();
  else if(env.tick==sett.ticksPerTime)
	  setHalfTime();

  if(env.tick<(sett.ticksPerTime*2)){
    step();
    return true;
  }
  else if(env.tick==(sett.ticksPerTime*2)){
      history.envs.back()=env;  // log also final environment
      env.tick++;
      env.state=R2State::Ended;
	}
  else if(env.tick>(sett.ticksPerTime*2)){  //just in case is reached by setEnvironment()
      env.state=R2State::Ended;
	}
  return false;
}

void R2Simulator::processStep(const R2Action& action, int team, int player){
  history.actions[env.tick][processedActions].team=team;
  history.actions[env.tick][processedActions].action=action;
  history.actions[env.tick][processedActions].player=player;
  processedActions++;
 
  switch(env.state)
  {
    case R2State::Inactive: 
      break;
    case R2State::Ready:  // currently, an unnecessary state
      procReady(action, team, player); 
      break;
    case R2State::Kickoff1:
    case R2State::Kickoff2:
      procKickoff(action, team, player);
      break;
    case R2State::Play:
      procPlay(action, team, player);
      break;
    case R2State::Stopped:
      break;
    case R2State::Goalkick1up:
    case R2State::Goalkick1down:
    case R2State::Goalkick2up:
    case R2State::Goalkick2down:
      procGoalkick(action, team, player);
      break;
    case R2State::Corner1up:
    case R2State::Corner1down:
    case R2State::Corner2up:
    case R2State::Corner2down:
      procCorner(action, team, player);
      break;
    case R2State::Throwin1:
    case R2State::Throwin2:
      procThrowin(action, team, player);
      break;
    case R2State::Paused:
      break;
    case R2State::Halftime:
      break;
    case R2State::Goal1:
    case R2State::Goal2:
      procGoal(action, team, player);
      break;
    case R2State::Ended:
      procEnded(action, team, player);
      break;
    default:
      break;
  } 
}

// we check the center of the body of the player
void R2Simulator::limitPlayersOutsideArea(int kickTeam){
  int i=0;
  int team=1-kickTeam;
  double defaultX= ((kickTeam && (!env.halftimePassed) ) || ( (!kickTeam) && env.halftimePassed) ) ? pitch.areaRx : pitch.areaLx;
  for(auto& p: env.teams[team]){
    if(isPlayerInsideOpponentArea(i, team)){
      p.pos.x=defaultX;
    }
    i++;
  }
}

void R2Simulator::limitPlayersOutsideAreaFullBody(int kickTeam){
  int i=0;
  int team=1-kickTeam;
  double defaultX= ((kickTeam && (!env.halftimePassed) ) || ( (!kickTeam) && env.halftimePassed) )? (pitch.areaRx-sett.playerRadius) : (pitch.areaLx+sett.playerRadius);
  for(auto& p: env.teams[team]){
    if(isPlayerInsideOpponentAreaFullBody(i, team)){
      p.pos.x=defaultX;
    }
    i++;
  }
}

// we check the complete diameter of the body of the player
void R2Simulator::limitPlayersToHalfPitch(int kickTeam, double dueDistance){
  for(auto& p: env.teams[0]){
    if (!env.halftimePassed){
      if (p.pos.x >0.0)
        p.pos.x=0.0;
    }
    else{
      if(p.pos.x <0.0)
        p.pos.x=0.0;
    }
  }

  for(auto&p: env.teams[1]){
    if (!env.halftimePassed){
      if(p.pos.x <0.0)
        p.pos.x=0.0;
    }
    else{
      if (p.pos.x >0.0)
        p.pos.x=0.0;
    }
  }
  
  for(auto& p: env.teams[1-kickTeam]){
    double dist=p.absDistanceFromCenter();
    if(dist<=R2Epsilon){
      p.pos.x=(-2*kickTeam+1)*dueDistance;  //to have sign - only if kickteam is 1
      p.pos.y=(uniformDist(rng)-0.5)*dueDistance;
    }
    else if (dist < dueDistance)
    {
      double ratio=dueDistance/dist;
      p.pos.x*=ratio;
      p.pos.y*=ratio;
    }
  }
  
}

void R2Simulator::procReady(const R2Action& action, int team, int player){
  switch(action.action){
    case  R2ActionType::NoOp:
      break;
    case  R2ActionType::Move:
      actionMove(action, team, player);
      break;
    case  R2ActionType::Dash:
      break;
    case  R2ActionType::Turn:
      break;
    case  R2ActionType::Kick:
      break;
    case  R2ActionType::Catch:
      break;
    default:
      break;
  }
}

void R2Simulator::procKickoff(const R2Action& action, int team, int player){
  switch(action.action){
    case  R2ActionType::NoOp:
      break;
    case  R2ActionType::Move:
      actionMoveKickoff(action, team, player);
      break;
    case  R2ActionType::Dash:
      break;
    case  R2ActionType::Turn:
      break;
    case  R2ActionType::Kick:
      break;
    case  R2ActionType::Catch:
      break;
    default:
      break;
  }
}

void R2Simulator::procPlay(const R2Action& action, int team, int player){
  switch(action.action){
    case  R2ActionType::NoOp:
      break;
    case  R2ActionType::Move:
      break;
    case  R2ActionType::Dash:
      actionDash(action, team, player);
      break;
    case  R2ActionType::Turn:
      break;
    case  R2ActionType::Kick:
      actionKick(action, team, player);
      break;
    case  R2ActionType::Catch:
      actionCatch(action, team, player);
      break;
    default:
      break;
  }
}

void R2Simulator::procGoalkick(const R2Action& action, int team, int player){
  switch(action.action){
    case  R2ActionType::NoOp:
      break;
    case  R2ActionType::Move:
      actionMoveGoalkick(action, team, player);
      break;
    case  R2ActionType::Dash:
      break;
    case  R2ActionType::Turn:
      break;
    case  R2ActionType::Kick:
      break;
    case  R2ActionType::Catch:
      break;
    default:
      break;
  }
}

void R2Simulator::procCorner(const R2Action& action, int team, int player){
  switch(action.action){
    case  R2ActionType::NoOp:
      break;
    case  R2ActionType::Move:
      actionMoveCorner(action, team, player);
      break;
    case  R2ActionType::Dash:
      break;
    case  R2ActionType::Turn:
      break;
    case  R2ActionType::Kick:
      break;
    case  R2ActionType::Catch:
      break;
    default:
      break;
  }
}

void R2Simulator::procThrowin(const R2Action& action, int team, int player){
  switch(action.action){
    case  R2ActionType::NoOp:
      break;
    case  R2ActionType::Move:
      actionMoveThrowin(action, team, player);
      break;
    case  R2ActionType::Dash:
      break;
    case  R2ActionType::Turn:
      break;
    case  R2ActionType::Kick:
      break;
    case  R2ActionType::Catch:
      break;
    default:
      break;
  }
}

void R2Simulator::procGoal(const R2Action& action, int team, int player){
  switch(action.action){
    case  R2ActionType::NoOp:
      break;
    case  R2ActionType::Move:
      break;
    case  R2ActionType::Dash:
      break;
    case  R2ActionType::Turn:
      break;
    case  R2ActionType::Kick:
      break;
    case  R2ActionType::Catch:
      break;
    default:
      break;
  }
}

void R2Simulator::procEnded(const R2Action& action, int team, int player){
  switch(action.action){
    case  R2ActionType::NoOp:
      break;
    case  R2ActionType::Move:
      break;
    case  R2ActionType::Dash:
      break;
    case  R2ActionType::Turn:
      break;
    case  R2ActionType::Kick:
      break;
    case  R2ActionType::Catch:
      break;
    default:
      break;
  }
}

std::vector<std::string> R2Simulator::getTeamNames(){
  return std::vector<std::string>{ teamNames[0], teamNames[1]};
}

std::string R2Simulator::getStateString(){
  std::string s= teamNames[0]+(env.halftimePassed ? " (right) " : " (left) ")+ "vs " + teamNames[1]+(env.halftimePassed ? " (left) " : " (right) ")+ to_string(env.score1)+"-"+to_string(env.score2)+" "+"tick:"+to_string(env.tick)+" ";
  switch(env.state)  {
    case R2State::Inactive: 
      return s+string("Inactive");
    case R2State::Ready:  // currently, an unnecessary state
      return s+string("Ready");
    case R2State::Kickoff1:
      return s+string("Kickoff1");
    case R2State::Kickoff2:
      return s+string("Kickoff2");
    case R2State::Play:
      return s+string("Play");
    case R2State::Stopped:
      return s+string("Stopped");
    case R2State::Goalkick1up:
      return s+string("Goalkick1up");
    case R2State::Goalkick1down:
      return s+string("Goalkick1down");
    case R2State::Goalkick2up:
      return s+string("Goalkick2up");
    case R2State::Goalkick2down:
      return s+string("Goalkick2down");
    case R2State::Corner1up:
      return s+string("Corner1up");
    case R2State::Corner1down:
      return s+string("Corner1down");
    case R2State::Corner2up:
      return s+string("Corner2up");
    case R2State::Corner2down:
      return s+string("Corner2down");
    case R2State::Throwin1:
      return s+string("Throwin1");
    case R2State::Throwin2:
      return s+string("Throwin2");
    case R2State::Paused:
      return s+string("Paused");
    case R2State::Halftime:
      return s+string("Halftime");
    case R2State::Goal1:
      return s+string("Goal1");
    case R2State::Goal2:
      return s+string("Goal2");
    case R2State::Ended:
      return s+string("Ended");
    default:
      return s+string("Unknown");
  }
}
 
std::string R2Simulator::createDateFilename(){
    char string_buf[90];
    time_t simpletime;
    struct tm * timeloc;

    time (&simpletime);
    timeloc = localtime(&simpletime);

    strftime(string_buf, sizeof(string_buf),"%Y-%m-%d %H-%M-%S",timeloc);
    std::string filename(string_buf);
    sprintf(string_buf,"_%u", random_seed);
    filename.append(string_buf);
    return filename;
} 

bool R2Simulator::saveStatesHistory(std::string filename){
  ofstream myfile;
  myfile.open (filename);
  if (!myfile.is_open())
    return false;

  myfile << R2SVersion << std::endl;

  myfile << teamNames[0] << std::endl;
  myfile << teamNames[1] << std::endl;

  myfile << env.teams[0].size() << "," << env.teams[1].size() << std::endl;

  myfile << sett.ticksPerTime;
  myfile << "," << sett.pitchLength;
  myfile << "," << sett.pitchWidth;
  myfile << "," << sett.goalWidth;
  myfile << "," << sett.centerRadius;
  myfile << "," << sett.poleRadius;   
  myfile << "," << sett.ballRadius;
  myfile << "," << sett.playerRadius;
  myfile << "," << sett.catchRadius;
  myfile << "," << sett.catchHoldingTicks;
  myfile << "," << sett.kickRadius;
  myfile << "," << sett.kickableDistance;
  myfile << "," << sett.catchableDistance;
  myfile << "," << sett.kickableAngle;
  myfile << "," << sett.kickableDirectionAngle;
  myfile << "," << sett.catchableAngle;
  myfile << "," << sett.netLength;
  myfile << "," << sett.catchableAreaLength;
  myfile << "," << sett.catchableAreaWidth;
  myfile << "," << sett.cornerMinDistance;
  myfile << "," << sett.throwinMinDistance;
  myfile << "," << sett.outPitchLimit;
  myfile << "," << sett.maxDashPower;
  myfile << "," << sett.maxKickPower;
  myfile << "," << sett.playerVelocityDecay;
  myfile << "," << sett.ballVelocityDecay;
  myfile << "," << sett.maxPlayerSpeed;
  myfile << "," << sett.maxBallSpeed;
  myfile << "," << sett.catchProbability;
  myfile << "," << sett.playerRandomNoise;
  myfile << "," << sett.playerDirectionNoise;
  myfile << "," << sett.playerVelocityDirectionMix;
  myfile << "," << sett.ballInsidePlayerVelocityDisplace;
  myfile << "," << sett.afterCatchDistance;
  myfile << std::endl;

  for (int tick=0; tick<history.envs.size(); tick++){
    auto& env=history.envs[tick];

    myfile << tick << ",";
    myfile << env.score1 << ",";
    myfile << env.score2 << ",";
    myfile << int(env.state) << ",";

    myfile << env.ball.pos.x << "," << env.ball.pos.y << "," << env.ball.velocity.x << "," << env.ball.velocity.y << "," ;

    for(auto& p1: env.teams[0])
      myfile << p1.pos.x << "," << p1.pos.y << "," << p1.velocity.x << "," << p1.velocity.y << "," << p1.direction << "," ;
    
    for(auto& p2: env.teams[1])
      myfile << p2.pos.x << "," << p2.pos.y << "," << p2.velocity.x << "," << p2.velocity.y << "," << p2.direction << "," ;

    myfile << env.lastTouchedTeam2 << ",";
    myfile << env.startingTeamMaxRange << ",";
    myfile << env.ballCatched << ",";
    myfile << env.ballCatchedTeam2;

    myfile << std::endl;
  }

  myfile.close();
  return true;
}

bool R2Simulator::saveActionsHistory(std::string filename){
  ofstream myfile;
  myfile.open (filename);
  if (!myfile.is_open())
    return false;

  for (int tick=0; tick<history.actions.size(); tick++)
    for(auto& actionPack : history.actions[tick]){
      myfile << tick << "," << actionPack.team << "," << actionPack.player << "," << int(actionPack.action.action) << "," 
        << actionPack.action.data[0] << "," << actionPack.action.data[1] << "," << actionPack.action.data[2] << std::endl;
    }

  myfile.close();
  return true;
}


void R2Simulator::setEnvironment(int _tick, int _score1, int _score2, R2State _state, R2ObjectInfo _ball, 
    std::vector<R2PlayerInfo> _team1, std::vector<R2PlayerInfo> _team2,
    bool _lastTouchedTeam2, int _ballCatched, bool _ballCatchedTeam2) {
        if (_tick >= (2*sett.ticksPerTime))
            _tick=2*sett.ticksPerTime;

        env.tick=_tick;
        env.score1=_score1;
        env.score2=_score2;
        env.state=_state;
        env.ball=_ball;
        env.teams[0]=_team1;
        env.teams[1]=_team2;
        env.lastTouchedTeam2=_lastTouchedTeam2;
        env.ballCatched=_ballCatched;
        env.ballCatchedTeam2=_ballCatchedTeam2;

        if(env.tick>=sett.ticksPerTime)
            env.halftimePassed=true;

        oldEnv = env;
        oldEnv.state= R2State::Inactive;
  }

} //end namespace

// (c) 2021 Ruggero Rossi
// a simple, reactive, robosoc2d player agent (with no planning)
#ifndef R2S_SIMPLEPLAYER_H
#define R2S_SIMPLEPLAYER_H

namespace r2s {

class SimplePlayer : public R2Player {
private:
    int shirtNumber;
    int team;
    R2EnvSettings sett;
    R2Environment normalEnv;
    R2Environment env;  // to have the environmente always rotated as if your team is teams[0] on the left
    R2Pitch pitch;
    R2Action prevAction;
    R2State prevState;
    std::uniform_real_distribution<double> uniformDist;
    std::default_random_engine rng;
    double cosKickableAngle;
    double cosCatchableAngle;

    double minOpponentSegmentDistance(Vec2 s1, Vec2 s2);
    Vec2 calcAdvancedPosition(int player);
    double companionPassValue(int player, bool advanced);
    double companionPassValue(int player);
    double companionPassValueAdvanced(int player);
    std::tuple<Vec2, double> choosePass();
    void transformEnvIfNecessary();
    R2Action  transformActionIfNecessary(R2Action action);
    double bestKickAnglePossible(double angle);
    std::tuple<bool, double>  isKickOrientedTowardOwnGoal(double angle);
    double angleTowardPoint(double x, double y);
    double angleTowardPoint(Vec2 p) { return angleTowardPoint(p.x, p.y); }
    double angleTowardBall();
    double angleTowardCenterGoal();
    double angleTowardUpperPole();
    double angleTowardLowerPole();
    double angleTowardOwnUpperPole();
    double angleTowardOwnLowerPole();
    double ballAngleTowardPoint(double x, double y);
    double ballAngleTowardCompanion(int companion);
    double ballAngleTowardCenterGoal();
    double ballAngleTowardUpperPole();
    double ballAngleTowardLowerPole();
    bool isTeamResumeKicking();
    bool isBallKickable();
    bool isOpponentCloserToBall();
    Vec2 opponentBaricenter();

    int playerClosestToBall(std::vector<R2PlayerInfo> & team);
    int myPlayerClosestToBall();
    int opponentPlayerClosestToBall();
    int myClosestOpponent();
    int myClosestCompanion();
    int myPlayerClosestToOpponent(int opponent, std::vector<int> assigned);

    R2Action chooseBestThrowin();
    R2Action chooseBestCornerKick();
    R2Action chooseBestAttackKick();
    R2Action chooseBestPersonalBallAdvancing();
    R2Action chooseBestPassOrAdvancing();
    R2Action playStub();
    R2Action play();
    R2Action playGoalkeeper();
    R2Action positionPlayer();
    R2Action positionGoalkeeper();

public:
    SimplePlayer(int index, int _whichTeam=0);
    ~SimplePlayer(){}; 

    virtual R2Action step(const R2GameState gameState) override;
};

std::unique_ptr<R2Simulator> buildSimplePlayerTwoTeamsSimulator(int nPlayers1, int nPlayers2,
    std::string team1name=defaultTeam1Name, std::string team2name=defaultTeam2Name,
    unsigned int random_seed= createChronoRandomSeed(),
    R2EnvSettings settings = R2EnvSettings());

std::unique_ptr<R2Simulator> buildSimplePlayerOneTeamSimulator(int nSimplePlayers, int simpleTeamNumber, std::vector<std::shared_ptr<R2Player>> otherTeam,
    std::string team1name=defaultTeam1Name, std::string team2name=defaultTeam2Name,
    unsigned int random_seed= createChronoRandomSeed(),
    R2EnvSettings settings = R2EnvSettings()); 
} // end namespace

#endif // R2S_SIMPLEPLAYER_H


// (c) 2021 Ruggero Rossi
// a simple, reactive, robosoc2d player agent (with no planning)
#include <iostream>


using namespace std;
using namespace r2s;

// explanation: http://paulbourke.net/geometry/pointlineplane/
// p is the point
// s1 and s2 are the segment's ends
double distancePointSegment(Vec2 p, Vec2 s1, Vec2 s2)
{
    Vec2 d=s2-s1;
    Vec2 dp1=p-s1; 

    //if the segment is collapsed in one point we can't use the formula but we can still calculate point to point distance
    if ( (d.x >= -R2SmallEpsilon) && (d.x <= R2SmallEpsilon) &&
        (d.y >= -R2SmallEpsilon) && (d.y <= R2SmallEpsilon) )
        return dp1.len();
    
    double l=d.len();
    double u= (dp1.x*d.x + dp1.y*d.y)/(l*l);

    if( (u<0.0) || (u>1.0) ){   // perpendicular intersection would be outside segment. Take instead the shorter value among the two distances between the point and the two segment's ends 
        Vec2 dp2=p-s2;
        return min(dp1.len(), dp2.len());
    }

    Vec2 intersection(s1.x+u*d.x , s1.y+u*d.y);
    return (p-intersection).len();
}

namespace r2s {

SimplePlayer::SimplePlayer(int index, int _whichTeam) : shirtNumber(index), team(_whichTeam),
 normalEnv(), env(), prevAction(), prevState(R2State::Inactive),
 uniformDist(0.0, 1.0), rng (42), cosKickableAngle(0.0), cosCatchableAngle(0.0) {
};

R2Action  SimplePlayer::transformActionIfNecessary(R2Action action){
    if((team && (!env.halftimePassed) ) || ( (!team) && env.halftimePassed)){
        switch(action.action){
            case R2ActionType::Move :
                action.data[0]=-action.data[0];
                action.data[1]=-action.data[1];
                action.data[2]=fixAnglePositive(action.data[2] + M_PI);
            break;            
            case R2ActionType::Dash :
            case R2ActionType::Turn :
            case R2ActionType::Kick :
            case R2ActionType::Catch :
                action.data[0]=fixAnglePositive(action.data[0] + M_PI);
            break;
            default:
            break;
        }
    }
    return action;
}

void  SimplePlayer::transformEnvIfNecessary(){
    env=normalEnv;
    if(team) {
        env.teams[0]=normalEnv.teams[1];  // just in case they have a different number of players in the team
        env.teams[1]=normalEnv.teams[0];
        
        env.score1=normalEnv.score2;
        env.score2=normalEnv.score1;
        env.lastTouchedTeam2=!env.lastTouchedTeam2; // teams[1] thinks to be teams[0]
        env.ballCatchedTeam2=!env.ballCatchedTeam2;
        // now invert the state
        R2State state=env.state;
        switch(state){
            case R2State::Kickoff1 :
                env.state=R2State::Kickoff2 ;
            break;
            case R2State::Kickoff2 :
                env.state=R2State::Kickoff1 ;
            break;
            case R2State::Goalkick1up :
                env.state=R2State::Goalkick2down ;
            break;
            case R2State::Goalkick1down :
                env.state=R2State::Goalkick2up ;
            break;
            case R2State::Goalkick2up :
                env.state=R2State::Goalkick1down ;
            break;
            case R2State::Goalkick2down :
                env.state=R2State::Goalkick1up ;
            break;
            case R2State::Corner1up :
                env.state=R2State::Corner2down ;
            break;
            case R2State::Corner1down :
                env.state=R2State::Corner2up ;
            break;
            case R2State::Corner2up :
                env.state=R2State::Corner1down ;
            break;
            case R2State::Corner2down :
                env.state=R2State::Corner1up ;
            break;
            case R2State::Throwin1 :
                env.state=R2State::Throwin2 ;
            break;
            case R2State::Throwin2 :
                env.state=R2State::Throwin1 ;
            break;
            case R2State::Goal1:
                env.state=R2State::Goal2 ;
            break;
            case R2State::Goal2 :
                env.state=R2State::Goal1 ;
            break;
            default:
            break;
        }
    }

    if(env.halftimePassed) {
        R2State state=env.state;
        switch(state){
            case R2State::Goalkick1up :
                env.state=R2State::Goalkick1down ;
            break;
            case R2State::Goalkick1down :
                env.state=R2State::Goalkick1up ;
            break;
            case R2State::Goalkick2up :
                env.state=R2State::Goalkick2down ;
            break;
            case R2State::Goalkick2down :
                env.state=R2State::Goalkick2up ;
            break;
            case R2State::Corner1up :
                env.state=R2State::Corner1down ;
            break;
            case R2State::Corner1down :
                env.state=R2State::Corner1up ;
            break;
            case R2State::Corner2up :
                env.state=R2State::Corner2down ;
            break;
            case R2State::Corner2down :
                env.state=R2State::Corner2up ;
            break;
            default:
            break;
        }

    }

    if((team && (!env.halftimePassed) ) || ( (!team) && env.halftimePassed)){
        env.ball.pos.invert();
        env.ball.velocity.invert();
        for(int i=0; i<env.teams[0].size(); i++){
            env.teams[0][i].pos.invert();
            env.teams[0][i].velocity.invert();
            env.teams[0][i].direction=fixAnglePositive(env.teams[0][i].direction + M_PI);
        }
        for(int i=0; i<env.teams[1].size(); i++){
            env.teams[1][i].pos.invert();
            env.teams[1][i].velocity.invert();
            env.teams[1][i].direction=fixAnglePositive(env.teams[1][i].direction + M_PI);
        }
    }

}

double SimplePlayer::bestKickAnglePossible(double angle){
    double bestAngle=angle;
    if( (!isTeamResumeKicking()) && (!sett.simplified) ) {
        auto& p=env.teams[0][shirtNumber];
        double a=fixAnglePositive(angle);
        double d=fixAngleTwoSides(a-p.direction);
        if(d > sett.kickableDirectionAngle){
            bestAngle=p.direction+sett.kickableDirectionAngle;   
        }
        else if(d < -sett.kickableDirectionAngle){
            bestAngle=p.direction-sett.kickableDirectionAngle;
        }
    } 
    return bestAngle;
}

std::tuple<bool, double> SimplePlayer::isKickOrientedTowardOwnGoal(double angle){
    double a=fixAnglePositive(angle);
    if( (a<M_PI_2) || (a> M_PI*1.5))
        return std::tuple<bool, double>{false, 0.0};

    Vec2 dir=Vec2(a);
    if(dir.x==0.0)
        return std::tuple<bool, double>{false, 0.0};
    
    double y=env.ball.pos.y+ (pitch.x2- env.ball.pos.x)*dir.y/dir.x;
    if( (y<pitch.yGoal1) && (y>pitch.yGoal2) )
        return std::tuple<bool, double>{true, y};

    return std::tuple<bool, double>{false, 0.0};
}

double SimplePlayer::angleTowardPoint(double x, double y){
    auto& p= env.teams[0][shirtNumber];
    double dx=x - p.pos.x;
    double dy=y - p.pos.y;
    if((dx==0.0)&&(dy==0.0))
        return 0.0;
    return atan2(dy,dx);
}

double SimplePlayer::angleTowardBall(){
    return angleTowardPoint(env.ball.pos.x, env.ball.pos.y);
}

double SimplePlayer::angleTowardCenterGoal(){
    return angleTowardPoint(pitch.x1, 0.0);
}

double SimplePlayer::angleTowardUpperPole(){
    return angleTowardPoint(pitch.x1, pitch.yGoal1);
}

double SimplePlayer::angleTowardLowerPole(){
    return angleTowardPoint(pitch.x1, pitch.yGoal2);
}

double SimplePlayer::angleTowardOwnUpperPole(){
    return angleTowardPoint(pitch.x2, pitch.yGoal1);
}

double SimplePlayer::angleTowardOwnLowerPole(){
    return angleTowardPoint(pitch.x2, pitch.yGoal2);
}

double SimplePlayer::ballAngleTowardPoint(double x, double y){
    double dx=x - env.ball.pos.x;
    double dy=y - env.ball.pos.y;
    if((dx==0.0)&&(dy==0.0))
        return 0.0;
    return atan2(dy,dx);
}

double SimplePlayer::ballAngleTowardCompanion(int companion){
    return ballAngleTowardPoint(env.teams[0][companion].pos.x, env.teams[0][companion].pos.y);
}

double SimplePlayer::ballAngleTowardCenterGoal(){
    return ballAngleTowardPoint(pitch.x1, 0.0);
}

double SimplePlayer::ballAngleTowardUpperPole(){
    return ballAngleTowardPoint(pitch.x1, pitch.yGoal1);
}

double SimplePlayer::ballAngleTowardLowerPole(){
    return ballAngleTowardPoint(pitch.x1, pitch.yGoal2);
}

bool SimplePlayer::isTeamResumeKicking(){
    return ((prevState==R2State::Kickoff1) || (prevState==R2State::Goalkick1up) ||
     (prevState==R2State::Goalkick1down) || (prevState==R2State::Throwin1) ||
     (prevState==R2State::Corner1up) || (prevState==R2State::Corner1down) );
}

bool SimplePlayer::isBallKickable(){
    auto p= env.teams[0][shirtNumber]; 
    
    auto [dist, d]=p.dist(env.ball); 
    // is ball reachable
    if(dist>sett.kickableDistance)
        return false;

    // is ball in front of player? only when not in kicking from restart game.
    if( (!sett.simplified) && (!isTeamResumeKicking()) && (fabs(dist)>R2SmallEpsilon) ){
        double cosinusPlayerBall= (d.x*cos(p.direction) + d.y*sin(p.direction))/dist;
        if(cosinusPlayerBall <= cosKickableAngle)  
            return false;
    }

    return true;
}

// not considering the goalkeeper
Vec2 SimplePlayer::opponentBaricenter(){
    Vec2 sum;
    int i=1;
    for (; i<env.teams[1].size(); i++){
        sum+=env.teams[1][i].pos;
    }
    return sum*(1/i);
}

// except goalkeeper
bool SimplePlayer::isOpponentCloserToBall(){

    int closerTeam=0;
    double minDist=1000000.0;
    for(int team=0; team<=1; team++)
        for (int i=1; i<env.teams[team].size(); i++){
            auto diff=env.teams[team][i].pos - env.ball.pos;
            double dist=diff.x*diff.x+diff.y*diff.y;
            if(dist<minDist){
                closerTeam=team;
                minDist=dist;
            }
        }
    return (closerTeam==1);
}

// except goalkeeper
int SimplePlayer::playerClosestToBall(std::vector<R2PlayerInfo>& team){
    int closest =1;
    if (team.size() < 2)
        return 0;
    auto diff1=team[1].pos - env.ball.pos;
    double minDist= diff1.x*diff1.x+diff1.y*diff1.y;
    for (int i=2; i<team.size(); i++){
        auto diff=team[i].pos - env.ball.pos;
        double dist=diff.x*diff.x+diff.y*diff.y;
        if(dist<minDist){
            closest=i;
            minDist=dist;
        }
    }
    return closest;
}

// except goalkeeper
int SimplePlayer::myPlayerClosestToBall(){
    return playerClosestToBall(env.teams[0]);
}

// except goalkeeper
int SimplePlayer::opponentPlayerClosestToBall(){
    return playerClosestToBall(env.teams[1]);
}

//be careful: returns -1 if opposing team is empty
//currently unused method
int SimplePlayer::myClosestOpponent(){
    if(env.teams[1].size()<1)
        return -1;
    auto& myself=env.teams[0][shirtNumber];
    int closest =0;
    auto diff1=env.teams[1][0].pos - myself.pos;
    double minDist= diff1.x*diff1.x+diff1.y*diff1.y;
    for (int i=1; i<env.teams[1].size(); i++){
        auto diff=env.teams[1][i].pos - myself.pos;
        double dist=diff.x*diff.x+diff.y*diff.y;
        if(dist<minDist){
            closest=i;
            minDist=dist;
        }
    }
    return closest;
}

//be careful: returns -1 if its the only player in the team
//currently unused method
int SimplePlayer::myClosestCompanion(){
    auto& myself=env.teams[0][shirtNumber];
    int closest = -1;
    double minDist=1000000.0;
    for (int i=0; i<env.teams[0].size(); i++){
        if(i!=shirtNumber){
            auto diff=env.teams[0][i].pos - myself.pos;
            double dist=diff.x*diff.x+diff.y*diff.y;
            if(dist<minDist){
                closest=i;
                minDist=dist;
            }
        }
    }
    return closest;
}

int SimplePlayer::myPlayerClosestToOpponent(int opponent, std::vector<int> assigned){
    Vec2 pos=env.teams[1][opponent].pos;
    int closest =-1;
    double minDist=1000000.0;
    for (int i=0; i<assigned.size(); i++){
        if(assigned[i]==0){
            auto diff=pos - env.teams[0][i].pos;
            double dist=diff.x*diff.x+diff.y*diff.y;
            if(dist<minDist){
                closest=i;
                minDist=dist;
            }
        }
    }
    return closest;
}

double SimplePlayer::minOpponentSegmentDistance(Vec2 s1, Vec2 s2){
    double minSegmentDistance=1000.0;
    for(int i=0; i<env.teams[1].size(); i++){
        auto& p2=env.teams[1][i];
        double segmentDistance=distancePointSegment(p2.pos, s1, s2);
        minSegmentDistance=min(minSegmentDistance,segmentDistance);
    }
    return minSegmentDistance;
}

constexpr double passGoalkeeperDisadvantage=-10;
constexpr double passBackwardDisadvantage=-5;
constexpr double passAdvancedAdvantage=5;
constexpr double passTrajectoryOccludedDisadvantage=-20;
constexpr double passFavouriteDistance=10;
constexpr double passDistanceDisadvantage=-0.1;   
constexpr double advancedPassLength=5.0;
constexpr double outOfAngleDisadvantage=-25;
constexpr double ownGoalDistanceThreshold=6;
constexpr double ownGoalDistanceDisadvantage=-15;

Vec2 SimplePlayer::calcAdvancedPosition(int player){
    auto pos=env.teams[0][player].pos;
    Vec2 d=Vec2(pitch.x1, 0.0) - pos;
    d.resize(advancedPassLength); //segment from player to center of opposite goal
    pos+=d;
    return pos;
}

double SimplePlayer::companionPassValue(int player, bool advanced){
    auto pos= advanced ? calcAdvancedPosition(player) : env.teams[0][player].pos;
    double value=0.0;

    if(advanced){
        value+=passAdvancedAdvantage;
    }

    double minSegmentDistance=minOpponentSegmentDistance(pos, env.ball.pos);
    
    if(minSegmentDistance<= (sett.ballRadius+sett.playerRadius))
        value+=passTrajectoryOccludedDisadvantage;

    if(player==0)
        value+=passGoalkeeperDisadvantage;

    if(pos.x < env.ball.pos.x)
        value+=passBackwardDisadvantage;

    double dist= (env.ball.pos - pos).len();
    value+=fabs(passFavouriteDistance-dist)*passDistanceDisadvantage;

    // calc if kicking angle is possible
    bool outOfAngle=false;
    Vec2 d=pos -env.ball.pos;
    double angle=0.0;
    if( (!sett.simplified) && (d.x!=0.0)&&(d.y!=0.0)){
        angle=atan2(d.y,d.x);
        if(!isTeamResumeKicking()){
            auto& p=env.teams[0][shirtNumber];
            double a=fixAnglePositive(angle);
            double diff=fixAngleTwoSides(a-p.direction);
            if(diff > sett.kickableDirectionAngle){
                value+=outOfAngleDisadvantage;  
                outOfAngle=true; 
            }
            else if(diff < -sett.kickableDirectionAngle){
                value+=outOfAngleDisadvantage;   
                outOfAngle=true; 
            }
        } 
    }

    auto [towardsOwnGoal, towardsY] =isKickOrientedTowardOwnGoal(angle);
    if(towardsOwnGoal){
        if(outOfAngle){
            angle=bestKickAnglePossible(angle);
        }
        Vec2 ownGoal(pitch.x2, towardsY);
        double distGoal=(env.ball.pos-ownGoal).len();
        if( distGoal< ownGoalDistanceThreshold){
            double disadvantage= (ownGoalDistanceThreshold - distGoal)*ownGoalDistanceDisadvantage/ownGoalDistanceThreshold;
            value+=disadvantage;
        }
    }
    
    return value;
}

// value to pass to a companion in his still position
double SimplePlayer::companionPassValue(int player){
    return companionPassValue(player, false);
}

// value to pass to a companion in an advanced position
double SimplePlayer::companionPassValueAdvanced(int player){
    return companionPassValue(player, true);
}

std::tuple<Vec2, double> SimplePlayer::choosePass(){
    int nplayers=env.teams[0].size();
    
    double bestStill=-1000.0;
    int bestStillIndex=shirtNumber;
    double bestAdvanced=-1000.0;
    int bestAdvancedIndex=shirtNumber;
    for(int i=0; i<nplayers; i++){
        if(i!=shirtNumber){
            double valueStill=companionPassValue(i);
            if(bestStill<valueStill){
                bestStill=valueStill;
                bestStillIndex=i;
            }

            double valueAdvanced=companionPassValueAdvanced(i);
            if(bestAdvanced<valueAdvanced){
                bestAdvanced=valueAdvanced;
                bestAdvancedIndex=i;
            }

        }
    }

    if(bestStillIndex==shirtNumber){
        return std::tuple<Vec2, double>{Vec2(0.0, 0.0) , bestStill};
    }

    if(bestStill > bestAdvanced){
        return std::tuple<Vec2, double>{env.teams[0][bestStillIndex].pos , bestStill};
    }

    return std::tuple<Vec2, double>{calcAdvancedPosition(bestAdvancedIndex) , bestAdvanced};
}

R2Action SimplePlayer::chooseBestThrowin(){
    R2Action action;
    auto[pos, passValue]= choosePass();
    action= R2Action(R2ActionType::Kick, bestKickAnglePossible(ballAngleTowardPoint(pos.x,pos.y)), sett.maxKickPower);           
    return action;
}

// actually is the same as chooseBestThrowin(), but it can be improved for corner kick cases
R2Action SimplePlayer::chooseBestCornerKick(){
    R2Action action;
    auto[pos, passValue]= choosePass();
    action= R2Action(R2ActionType::Kick, bestKickAnglePossible(ballAngleTowardPoint(pos.x,pos.y)), sett.maxKickPower);   
    return action;
}

R2Action SimplePlayer::chooseBestAttackKick(){
    // check the best trajectory to kick the ball and do it
    int nIntervals=12;
    double bestY=0.0;
    double bestValue=0.0;
    for(int i=0; i<=nIntervals; i++){
        double py=pitch.yGoal1*(double(i)/nIntervals) + pitch.yGoal2*(double(nIntervals-i)/nIntervals);
        double distValue=minOpponentSegmentDistance(env.ball.pos, Vec2(pitch.x1, py));
        if(distValue>bestValue){
            bestValue=distValue;
            bestY=py;
        }
    }

    // not assured that actually you can kick that angle ! hence using bestKickAnglePossible()
    return R2Action(R2ActionType::Kick, bestKickAnglePossible(ballAngleTowardPoint(pitch.x1,bestY)), sett.maxKickPower);
}

constexpr double checkedDistance=3.0;
constexpr int nAdvancingIntervals=6;
constexpr double advancingIncentive=1.0;
constexpr double avoidOutIncentive=0.5;
constexpr double dangerBorderRatio=0.9;
constexpr double goalIncentive=50.0;
R2Action SimplePlayer::chooseBestPersonalBallAdvancing(){
    auto& p=env.teams[0][shirtNumber];
    double angleA=p.direction-sett.kickableAngle;
    double angleB=p.direction+sett.kickableAngle;
    double bestValue=0.0;
    double bestAngle=0.0;
    for(int i=0; i<=nAdvancingIntervals; i++){
        double angle=angleA*(double(i)/nAdvancingIntervals) + angleB*(double(nAdvancingIntervals-i)/nAdvancingIntervals);
        Vec2 target=Vec2(angle);
        target.resize(checkedDistance);
        target +=env.ball.pos;
        double value=minOpponentSegmentDistance(env.ball.pos, target);
        if(target.x > env.ball.pos.x)
            value+=advancingIncentive;
        double dangerBorder=pitch.y1*dangerBorderRatio;
        if(target.y > dangerBorder)
            value+=-(target.y-dangerBorder)/(pitch.y1*(1.0-dangerBorderRatio));
        else if(target.y < -dangerBorder)
            value+=(target.y-dangerBorder)/(pitch.y1*(1.0-dangerBorderRatio));
        double goalCloseness=goalIncentive*1.0/(1.0 + (Vec2(pitch.x1, 0.0)-target).len());
        double goalFactor=goalCloseness*goalCloseness;
        value+=goalFactor;

        auto [towardsOwnGoal, towardsY] =isKickOrientedTowardOwnGoal(angle);
        if(towardsOwnGoal){
            Vec2 ownGoal(pitch.x2, towardsY);
            double distGoal=(env.ball.pos-ownGoal).len();
            if( distGoal< ownGoalDistanceThreshold){
                double disadvantage= (ownGoalDistanceThreshold - distGoal)*ownGoalDistanceDisadvantage/ownGoalDistanceThreshold;
                value+=disadvantage;
            }
        }

        if(value>bestValue){
            bestValue=value;
            bestAngle=angle;
        }
    }
    return R2Action(R2ActionType::Kick, bestKickAnglePossible(bestAngle), sett.maxKickPower/2.0);
}

constexpr double minimumPassValue=0.1;
R2Action SimplePlayer::chooseBestPassOrAdvancing(){
    R2Action action;
    auto[pos, passValue]= choosePass();
    if(passValue>minimumPassValue){
        // pass the ball
        action= R2Action(R2ActionType::Kick, bestKickAnglePossible(ballAngleTowardPoint(pos.x,pos.y)), sett.maxKickPower);
    }
    else{
        action= chooseBestPersonalBallAdvancing();
    }
    return action;
}

constexpr double scoreDistance=8.0; // closer than this to goal => kick and try to score
R2Action SimplePlayer::play()
{
    R2Action action;

    if(isBallKickable()){
        if(prevState==R2State::Throwin1){
            action= chooseBestThrowin();
        }
        else if((prevState==R2State::Corner1down) || (prevState==R2State::Corner1up)){
            action= chooseBestCornerKick();
        }
        else{
            double goalDist=(env.ball.pos- Vec2(pitch.x1, 0.0)).len();
            if(goalDist<=scoreDistance){
                action= chooseBestAttackKick();
            }
            else{
                action=chooseBestPassOrAdvancing();
            }
        }
    }
    else{
        if(myPlayerClosestToBall()==shirtNumber){  // reach for the ball
            action= R2Action (R2ActionType::Dash, angleTowardPoint(env.ball.pos.x, env.ball.pos.y), sett.maxDashPower);
        }
        else{   // TODO decide if going back in defence, stay or advance. If out of the pitch, come back
            auto &pos=env.teams[0][shirtNumber].pos;
            if( pos.x > pitch.x1){
                if(pos.y < pitch.y2){   // out bottom right
                    action= R2Action (R2ActionType::Dash, M_PI*0.75, sett.maxDashPower);
                }
                else if(pos.y > pitch.y1){  // out top right
                    action= R2Action (R2ActionType::Dash, M_PI*1.25, sett.maxDashPower);
                }
                else{   //out right
                    action= R2Action (R2ActionType::Dash, M_PI, sett.maxDashPower);
                }
            }
            else if(pos.x < pitch.x2){
                if(pos.y < pitch.y2){   // out bottom left
                    action= R2Action (R2ActionType::Dash, M_PI*0.25, sett.maxDashPower);
                }
                else if(pos.y > pitch.y1){  // out top left
                    action= R2Action (R2ActionType::Dash, M_PI*1.75, sett.maxDashPower);
                }
                else{   //out left
                    action= R2Action (R2ActionType::Dash, 0.0, sett.maxDashPower);
                }
            }
            else if(pos.y > pitch.y1){  // out top
                action= R2Action (R2ActionType::Dash, M_PI*1.5, sett.maxDashPower);
            }
            else if(pos.y < pitch.y2){  // out bottom
                action= R2Action (R2ActionType::Dash, M_PI*0.5, sett.maxDashPower);
            }
            else{   // try to find your position
                if(shirtNumber!=0)
                    action= positionPlayer();
                else
                    action= positionGoalkeeper();
            }
        }
    }

    return action;
}

constexpr double markDistance=1.0;
R2Action SimplePlayer::positionPlayer(){
    const double goBackThresholdDistance=pitch.x2/2.0;
    const double goBackBaricenterThresholdDistance=pitch.x2/4.0;
    const double runForwardThresholdDistance=pitch.x1*2/3.0;

    R2Action action;

    if(isOpponentCloserToBall()){  //assume opponents are attacking

        if((env.ball.pos.x < goBackThresholdDistance) || (opponentBaricenter().dist(Vec2(pitch.x2,0.0))>goBackBaricenterThresholdDistance) ){
            // assign an opponent to mark. 0 is unassigned (you don't mark the goalkeepr), -1 is ball or no opponent. Goalkeeper is not marking, opponent goalkeeper is not marked
            std::vector<int> assignments(env.teams[0].size(), 0);
            int busy=myPlayerClosestToBall();    // this one is busy reaching for the ball
            assignments[busy]=-1;
            assignments[0]=-1;  //goalkeeper
            int busyOpponent=opponentPlayerClosestToBall();
            for(int i=1; i<env.teams[1].size(); i++){
                if(i!=busyOpponent){
                    int p=myPlayerClosestToOpponent(i, assignments);
                    if(p>0) // not assigning 0 (goal keeper) and -1 (no one)
                        assignments[p]=i;
                }
            }

            if(assignments[shirtNumber]< 0){   // not assigned (shouldn't be here if same number of players): reach for ball
                action= R2Action (R2ActionType::Dash, angleTowardBall(), sett.maxDashPower);
            }
            else if(assignments[shirtNumber] > 0){   // mark a player
                Vec2 target=env.teams[1][assignments[shirtNumber]].pos;
                Vec2 delta= Vec2(pitch.x2,0.0)-target;
                delta.resize(markDistance);
                target +=delta;
                action= R2Action (R2ActionType::Dash, angleTowardPoint(target), sett.maxDashPower);
            }

        }
        else{
            auto &pos=env.teams[0][shirtNumber].pos;
            double angle= (pos.y>0.0) ? angleTowardOwnUpperPole() : angleTowardOwnLowerPole();
            action= R2Action (R2ActionType::Dash, angle, sett.maxDashPower);
        }
    }
    else{
        auto &pos=env.teams[0][shirtNumber].pos;
        if(pos.x < runForwardThresholdDistance){
            double angle= (pos.y>0.0) ? angleTowardUpperPole() : angleTowardLowerPole();
            action= R2Action (R2ActionType::Dash, angle, sett.maxDashPower); 
        }
        else{   // try to stay in an unmarked position moving randomly
            double angle= uniformDist(rng)*2*M_PI;
            action= R2Action (R2ActionType::Dash, angle, sett.maxDashPower); 
        }
    }

    return action;
}

constexpr double goalkeeperInterventionDistance=3.0;
R2Action SimplePlayer::positionGoalkeeper(){
    R2Action action;
    auto& p= env.teams[0][shirtNumber];

    // if ball close, go for it
    if((p.pos.dist(env.ball.pos) < goalkeeperInterventionDistance) && (p.pos.x < 0.0) ){
        action= R2Action (R2ActionType::Dash, angleTowardBall(), sett.maxDashPower);
    }
    else{
        Vec2 goalCenter(pitch.x2, 0.0);
        Vec2 lineBallGoal=env.ball.pos-goalCenter;
        double wannaX= lineBallGoal.x/3.0;
        double wannaY= lineBallGoal.y/3.0;
        Vec2 wannaBe(pitch.x2+wannaX, wannaY);
        Vec2 posDiff=wannaBe-p.pos;

        if(posDiff.len()>0.5) {
            double angle=0.0;
            if(posDiff.x == 0.0){
                if(posDiff.y>0.0)
                    angle=M_PI_2;
                else
                    angle=-M_PI_2;
            }
            else {
                angle=atan2(posDiff.y, posDiff.x);
            }

            double dashPower=sett.maxDashPower;
            // to do: diminish dashPower when needed, to better adjust
            action= R2Action (R2ActionType::Dash, angle, dashPower);
        }
        else{   // if not moving, face the ball
            action= R2Action (R2ActionType::Dash, angleTowardBall(), 0.0);
        }
    }

    return action;
}

R2Action SimplePlayer::playGoalkeeper(){
    R2Action action;

    // 1) if ball is close and moving towards goal, catch it
    // 2) if ball is close and slow or still kick it to a companion (THE ONE THAT IS FAR FROM)
    // 3) otherwise goalkeeper wants to place himself on the line between the ball and the center of the goal
    //      choosing the distance depending on the most advanced opponent and on the ball
    
    auto p= env.teams[0][shirtNumber];
    auto [dist, d]=p.dist(env.ball); 

    double cosinusPlayerBall= (d.x*cos(p.direction) + d.y*sin(p.direction))/dist;

    if( (dist<sett.kickableDistance) &&
        (cosinusPlayerBall >= cosKickableAngle)  ){
        action= chooseBestPassOrAdvancing();    // actually if goalkeeper is holding the ball caught, this function is not accurate because the real ball position at the moment of kicking would not be the current in env (that is inside the goalkeeper) but instead it would be set right before the kick in front of the goalkeeper, at kick direction, at distance = sett.afterCatchDistance
    }
    else if( (dist< sett.catchableDistance ) &&    // is ball reachable ? then catch it
       ( (p.pos.x >= pitch.x2) && (p.pos.x <= pitch.areaLx) && (p.pos.y <= pitch.areaUy) && (p.pos.y >= pitch.areaDy) ) && // inside his area
       ( cosinusPlayerBall >= cosCatchableAngle)){ // -catchableAngle >= goalkeeper-ball angle <= catchableAngle 
       if(prevAction.action==R2ActionType::Kick){   
            //TODO: check if we need to catch it anyway
            return positionGoalkeeper();
       }
       action = R2Action(R2ActionType::Catch);
    }
    else {  // otherwise position yourself in the right way
       return positionGoalkeeper();
    }

    return action;
}


R2Action SimplePlayer::step(const R2GameState gameState) {
    sett=gameState.sett;
    cosKickableAngle=cos(sett.kickableAngle);
    cosCatchableAngle=cos(sett.catchableAngle);
    normalEnv=gameState.env;
    pitch=gameState.pitch;
    transformEnvIfNecessary();

    R2Action action;

    switch (env.state){
        case R2State::Inactive: 
            break;
        case R2State::Ready:  // currently, an unnecessary state
            break;
        case R2State::Kickoff1:{
            double x=-10.0,y=0.0;
            
            if (shirtNumber==0) {
                x=(pitch.x2+pitch.areaLx)/2;
                y=0.0;
            }
            else if(shirtNumber==1) {
                x=-sett.ballRadius-sett.playerRadius-R2Epsilon;
                y=0.0;
            }
            else if(shirtNumber==2) {
                x=-sett.ballRadius;
                y=sett.centerRadius;
            }
            else if(shirtNumber==3) {
                x=-pitch.x1/3;
                y=-sett.centerRadius;
            }
            else {   
                x=-pitch.x1+pitch.x1/(shirtNumber-1);
                y= sett.centerRadius*( (shirtNumber%3) -1);
            }
            
            action= R2Action(R2ActionType::Move, x, y);
            }
            break;
       
        case R2State::Kickoff2:{
            double x=-10.0,y=0.0;
            
            if (shirtNumber==0)
            {
                x=(pitch.x2+pitch.areaLx)/2;
                y=0.0;
            }
            else if(shirtNumber==1)
            {
                x=-sett.centerRadius-sett.playerRadius-R2Epsilon;
                y=0.0;
            }
            else if(shirtNumber==2)
            {
                x=pitch.x2/2;
                y=pitch.y2/2;
            }
            else if(shirtNumber==3)
            {
                x=pitch.x2/2;
                y=-pitch.y2/2;
            }
            else{
                x=-pitch.x1+pitch.x1/(shirtNumber-1);
                y= sett.centerRadius*( (shirtNumber%3) -1);
            }
        
            action= R2Action (R2ActionType::Move, x, y);
            }
            break;
        case R2State::Play:            
            if(shirtNumber==0)
                action= playGoalkeeper();
            else
                action= play();
            break;
        case R2State::Goalkick1up:  // make the goalkeeper kick
            if(shirtNumber==0){
                action= R2Action (R2ActionType::Move, pitch.goalKickLx-(sett.kickRadius/10+sett.playerRadius+sett.ballRadius), pitch.goalKickUy);
            }
            else {
                // look for the best place to stay
            }
            break;
        case R2State::Goalkick1down:
            if(shirtNumber==0){
                action= R2Action (R2ActionType::Move, pitch.goalKickLx-(sett.kickRadius/10+sett.playerRadius+sett.ballRadius), pitch.goalKickDy);
            }
            else {
                // look for the best place to stay
            }
            break;
        case R2State::Goalkick2up:  // reposition team
        case R2State::Goalkick2down:
            break;
        case R2State::Corner1up:    // the closest to the ball go there
            if(myPlayerClosestToBall()==shirtNumber){
                action= R2Action (R2ActionType::Move, pitch.x1, pitch.y1+(sett.kickRadius/10+sett.playerRadius+sett.ballRadius) );
            }
            break;
        case R2State::Corner1down:
            if(myPlayerClosestToBall()==shirtNumber){
                action= R2Action (R2ActionType::Move, pitch.x1, pitch.y2-(sett.kickRadius/10+sett.playerRadius+sett.ballRadius) );
            }
            break;
        case R2State::Corner2up:    // position close to the opponents
        case R2State::Corner2down:
            break;
        case R2State::Throwin1: // the closest to the ball go there
            if(myPlayerClosestToBall()==shirtNumber){
                double displace=sett.kickRadius/10+sett.playerRadius+sett.ballRadius;
                if(env.ball.pos.y<0.0)
                    displace*=-1;
                action= R2Action (R2ActionType::Move, env.ball.pos.x, env.ball.pos.y+displace);
            }
            break;
        case R2State::Throwin2: // position close to the opponents 
            break;
        case R2State::Paused:
            break;
        case R2State::Halftime:
            break;
        case R2State::Goal1:
        case R2State::Goal2:
            break;
        case R2State::Ended:
            break;
        default:
            break;
    }
    
    prevState = env.state;
    prevAction = transformActionIfNecessary(action);
    return prevAction;
}

std::unique_ptr<R2Simulator> buildSimplePlayerTwoTeamsSimulator(int nPlayers1, int nPlayers2, string _team1name, string _team2name, unsigned int random_seed, R2EnvSettings settings){ 
   return buildSimulator<SimplePlayer,SimplePlayer>(nPlayers1, nPlayers2, _team1name, _team2name, random_seed, settings);
}

std::unique_ptr<R2Simulator> buildSimplePlayerOneTeamSimulator(int nSimplePlayers, int simpleTeamNumber, std::vector<std::shared_ptr<R2Player>> otherTeam, string _team1name, string _team2name, unsigned int random_seed, R2EnvSettings settings){ 
    return buildOneTeamSimulator<SimplePlayer>(nSimplePlayers, simpleTeamNumber, otherTeam, _team1name, _team2name, random_seed, settings);
}

} // namespace



using namespace std;
using namespace r2s;

#include <chrono>
//using namespace std::chrono;
#include <cstring>
#include <iostream>
#include <unordered_map>
#include <string>
#include <sstream>

constexpr auto defaultPythonTeam1Name = "Python Team A";
constexpr auto defaultPythonTeam2Name = "Python Team B";

static bool isPythonActive=false;
static PyObject *R2Error;

struct R2SettingsObject{
    PyObject_HEAD
    bool simplified;
    int ticksPerTime;   
    double pitchLength;
    double pitchWidth;
    double goalWidth;
    double centerRadius;
    double poleRadius;   
    double ballRadius;
    double playerRadius;
    double catchRadius;
    int catchHoldingTicks;
    double kickRadius;
    double kickableDistance;
    double catchableDistance;
    double kickableAngle;
    double kickableDirectionAngle;
    double catchableAngle;
    double netLength;
    double catchableAreaLength;
    double catchableAreaWidth;
    double cornerMinDistance;
    double throwinMinDistance;
    double outPitchLimit;
    double maxDashPower;
    double maxKickPower;
    double playerVelocityDecay;
    double ballVelocityDecay;
    double maxPlayerSpeed;
    double maxBallSpeed;
    double catchProbability;
    double playerRandomNoise;
    double playerDirectionNoise;
    double playerVelocityDirectionMix;
    double ballInsidePlayerVelocityDisplace;
    double afterCatchDistance;
};

static PyObject *R2SettingsObject_repr(R2SettingsObject * obj){
    std::stringstream buffer;
    buffer << "{" 
        << "'simplified': " << obj->simplified << ", "
        << "'ticks_per_time': " << obj->ticksPerTime << ", "
        << "'pitch_length': " << obj->pitchLength << ", "
        << "'pitch_width': " << obj->pitchWidth << ", "
        << "'goal_width': " << obj->goalWidth << ", "
        << "'center_radius': " << obj->centerRadius << ", "
        << "'pole_radius': " << obj->poleRadius << ", "
        << "'ball_radius': " << obj->ballRadius << ", "
        << "'player_radius': " << obj->playerRadius << ", "
        << "'catch_radius': " << obj->catchRadius << ", "
        << "'catch_holding_ticks': " << obj->catchHoldingTicks << ", "
        << "'kick_radius': " << obj->kickRadius << ", "
        << "'kickable_distance': " << obj->kickableDistance << ", "
        << "'catchable_distance': " << obj->catchableDistance << ", "
        << "'kickable_angle': " << obj->kickableAngle << ", "
        << "'kickable_direction_angle': " << obj->kickableDirectionAngle << ", "
        << "'catchable_angle': " << obj->catchableAngle << ", "
        << "'net_length': " << obj->netLength << ", "
        << "'catchable_area_length': " << obj->catchableAreaLength << ", "
        << "'catchable_area_width': " << obj->catchableAreaWidth << ", "
        << "'corner_min_distance': " << obj->cornerMinDistance << ", "
        << "'throwin_min_distance': " << obj->throwinMinDistance << ", "
        << "'out_pitch_limit': " << obj->outPitchLimit << ", "
        << "'max_dash_power': " << obj->maxDashPower << ", "
        << "'max_kick_power': " << obj->maxKickPower << ", "
        << "'player_velocity_decay': " << obj->playerVelocityDecay << ", "
        << "'ball_velocity_decay': " << obj->ballVelocityDecay << ", "
        << "'max_player_speed': " << obj->maxPlayerSpeed << ", "
        << "'max_ball_speed': " << obj->maxBallSpeed << ", "
        << "'catch_probability': " << obj->catchProbability << ", "
        << "'player_random_noise': " << obj->playerRandomNoise << ", "
        << "'player_direction_noise': " << obj->playerDirectionNoise << ", "
        << "'player_velocity_direction_mix': " << obj->playerVelocityDirectionMix << ", "
        << "'ball_inside_player_velocity_displace': " << obj->ballInsidePlayerVelocityDisplace << ", "
        << "'after_catch_distance': " << obj->afterCatchDistance
        << "}" ;
    return PyUnicode_FromString(buffer.str().c_str());
}

static void
R2SettingsType_dealloc(R2SettingsObject * obj)
{
    Py_TYPE(obj)->tp_free((PyObject *) obj);
}

void fillR2SettingsObject(R2SettingsObject& target, const R2EnvSettings& source){
    target.simplified = source.simplified;
    target.ticksPerTime = source.ticksPerTime;
    target.pitchLength = source.pitchLength;
    target.pitchWidth = source.pitchWidth;
    target.goalWidth = source.goalWidth;
    target.centerRadius = source.centerRadius;
    target.poleRadius = source.poleRadius;   
    target.ballRadius = source.ballRadius;
    target.playerRadius = source.playerRadius;
    target.catchRadius = source.catchRadius;
    target.catchHoldingTicks = source.catchHoldingTicks;
    target.kickRadius = source.kickRadius;
    target.kickableDistance = source.kickableDistance;
    target.catchableDistance = source.catchableDistance;
    target.kickableAngle = source.kickableAngle;
    target.kickableDirectionAngle = source.kickableDirectionAngle;
    target.catchableAngle = source.catchableAngle;
    target.netLength = source.netLength;
    target.catchableAreaLength = source.catchableAreaLength;
    target.catchableAreaWidth = source.catchableAreaWidth;
    target.cornerMinDistance = source.cornerMinDistance;
    target.throwinMinDistance = source.throwinMinDistance;
    target.outPitchLimit = source.outPitchLimit;
    target.maxDashPower = source.maxDashPower;
    target.maxKickPower = source.maxKickPower;
    target.playerVelocityDecay = source.playerVelocityDecay;
    target.ballVelocityDecay = source.ballVelocityDecay;
    target.maxPlayerSpeed = source.maxPlayerSpeed;
    target.maxBallSpeed = source.maxBallSpeed;
    target.catchProbability = source.catchProbability;
    target.playerRandomNoise = source.playerRandomNoise;
    target.playerDirectionNoise = source.playerDirectionNoise;
    target.playerVelocityDirectionMix = source.playerVelocityDirectionMix;
    target.ballInsidePlayerVelocityDisplace = source.ballInsidePlayerVelocityDisplace;
    target.afterCatchDistance = source.afterCatchDistance;
}

// filling C++ settings from Python settings (the inverse of the above)
void fillR2Settings(R2EnvSettings& target, const R2SettingsObject& source){
    target.simplified = source.simplified;
    target.ticksPerTime = source.ticksPerTime;
    target.pitchLength = source.pitchLength;
    target.pitchWidth = source.pitchWidth;
    target.goalWidth = source.goalWidth;
    target.centerRadius = source.centerRadius;
    target.poleRadius = source.poleRadius;   
    target.ballRadius = source.ballRadius;
    target.playerRadius = source.playerRadius;
    target.catchRadius = source.catchRadius;
    target.catchHoldingTicks = source.catchHoldingTicks;
    target.kickRadius = source.kickRadius;
    target.kickableDistance = source.kickableDistance;
    target.catchableDistance = source.catchableDistance;
    target.kickableAngle = source.kickableAngle;
    target.kickableDirectionAngle = source.kickableDirectionAngle;
    target.catchableAngle = source.catchableAngle;
    target.netLength = source.netLength;
    target.catchableAreaLength = source.catchableAreaLength;
    target.catchableAreaWidth = source.catchableAreaWidth;
    target.cornerMinDistance = source.cornerMinDistance;
    target.throwinMinDistance = source.throwinMinDistance;
    target.outPitchLimit = source.outPitchLimit;
    target.maxDashPower = source.maxDashPower;
    target.maxKickPower = source.maxKickPower;
    target.playerVelocityDecay = source.playerVelocityDecay;
    target.ballVelocityDecay = source.ballVelocityDecay;
    target.maxPlayerSpeed = source.maxPlayerSpeed;
    target.maxBallSpeed = source.maxBallSpeed;
    target.catchProbability = source.catchProbability;
    target.playerRandomNoise = source.playerRandomNoise;
    target.playerDirectionNoise = source.playerDirectionNoise;
    target.playerVelocityDirectionMix = source.playerVelocityDirectionMix;
    target.ballInsidePlayerVelocityDisplace = source.ballInsidePlayerVelocityDisplace;
    target.afterCatchDistance = source.afterCatchDistance;
}

const char R2Settings_doc[]= "Object containing the settings of the simulation.\n\n\
The method copy() returns a binary copy of this object.\n\
Converting an object to string and then evaluating that string with eval()\
will result in a dictionary containing all the fields of the object.\n\
E.g. my_dict=eval(str(my_settings))\n\n\
";

static PyTypeObject R2SettingsType =
{
    PyVarObject_HEAD_INIT(NULL, 0)
};

static PyObject *
R2Settings_copy(R2SettingsObject *self, PyObject *Py_UNUSED(ignored))
{
    R2SettingsObject* target=PyObject_New(R2SettingsObject, &R2SettingsType);
    if(target==NULL)
    {
	    PyErr_SetString(R2Error, "unable to create settings object");
        return NULL;
    }
    target->simplified = self->simplified;
    target->ticksPerTime = self->ticksPerTime;
    target->pitchLength = self->pitchLength;
    target->pitchWidth = self->pitchWidth;
    target->goalWidth = self->goalWidth;
    target->centerRadius = self->centerRadius;
    target->poleRadius = self->poleRadius;   
    target->ballRadius = self->ballRadius;
    target->playerRadius = self->playerRadius;
    target->catchRadius = self->catchRadius;
    target->catchHoldingTicks = self->catchHoldingTicks;
    target->kickRadius = self->kickRadius;
    target->kickableDistance = self->kickableDistance;
    target->catchableDistance = self->catchableDistance;
    target->kickableAngle = self->kickableAngle;
    target->kickableDirectionAngle = self->kickableDirectionAngle;
    target->catchableAngle = self->catchableAngle;
    target->netLength = self->netLength;
    target->catchableAreaLength = self->catchableAreaLength;
    target->catchableAreaWidth = self->catchableAreaWidth;
    target->cornerMinDistance = self->cornerMinDistance;
    target->throwinMinDistance = self->throwinMinDistance;
    target->outPitchLimit = self->outPitchLimit;
    target->maxDashPower = self->maxDashPower;
    target->maxKickPower = self->maxKickPower;
    target->playerVelocityDecay = self->playerVelocityDecay;
    target->ballVelocityDecay = self->ballVelocityDecay;
    target->maxPlayerSpeed = self->maxPlayerSpeed;
    target->maxBallSpeed = self->maxBallSpeed;
    target->catchProbability = self->catchProbability;
    target->playerRandomNoise = self->playerRandomNoise;
    target->playerDirectionNoise = self->playerDirectionNoise;
    target->playerVelocityDirectionMix = self->playerVelocityDirectionMix;
    target->ballInsidePlayerVelocityDisplace = self->ballInsidePlayerVelocityDisplace;
    target->afterCatchDistance = self->afterCatchDistance;

    return (PyObject*)target;
}

static PyMethodDef R2Settings_methods[] = {
    {"copy", (PyCFunction) R2Settings_copy, METH_NOARGS,
     "Builds and returns a copy"
    },
    {NULL}  /* Sentinel */
};

static PyMemberDef R2Settings_members[] = {
    {(char*)"simplified", T_BOOL, offsetof(R2SettingsObject, simplified), 0, (char*)"boolean : set it to False if you want a more realistic ball control. Set it to True (default value) if you want a simplified simulation that eases the ball control to the agent and relaxes constraints on ball kicking (in this modality agents can kick the ball even behind their back towards any position, as if they can capture the ball from one side and release it from the other)"},  
    {(char*)"ticks_per_time", T_INT, offsetof(R2SettingsObject, ticksPerTime), 0, (char*)"integer : duration of each half time, expressed in ticks (a tick is a time step). The total duration of a match is 2*ticks_per_time (since there are 2 half times)"},
    {(char*)"pitch_length", T_DOUBLE, offsetof(R2SettingsObject, pitchLength), 0, (char*)"float : length of the pitch, that in the simulation extends along horizontal axes. pitch_length is the length of the playable zone, delimited by end lines. If during play the ball exit that zone there will be a goal-kick or a corner kick, but players can anyway move and dash outside that zone in a zone that is larger by the quantity out_pitch_limit (the actual coordinates are calculated in the object robosoc2d.pitch, in pitch.border_left and pitch.border_right"},
    {(char*)"pitch_width", T_DOUBLE, offsetof(R2SettingsObject, pitchWidth), 0, (char*)"float : width of the pitch, that in the simulation extends along vertical axes. pitch_width is the width of the playable zone, delimited by side lines. If during play the ball exit that zone there will be a throw-in, but players can anyway move and dash outside that zone in a zone that is larger by the quantity out_pitch_limit (the actual coordinates are calculated in the object robosoc2d.pitch, in pitch.border_up and pitch.border_down)"},
    {(char*)"goal_width", T_DOUBLE, offsetof(R2SettingsObject, goalWidth), 0, (char*)"float : width of goals, in vertical coordinates. It is the internal width, excluding poles. External width including poles instead is equal to (goal_width + (pole_radius*2)*2)"},
    {(char*)"center_radius", T_DOUBLE, offsetof(R2SettingsObject, centerRadius), 0, (char*)"float : radius of center circle of the pitch"},
    {(char*)"pole_radius", T_DOUBLE, offsetof(R2SettingsObject, poleRadius), 0, (char*)"float : radius of poles"},   
    {(char*)"ball_radius", T_DOUBLE, offsetof(R2SettingsObject, ballRadius), 0, (char*)"float : radius of the ball"},
    {(char*)"player_radius", T_DOUBLE, offsetof(R2SettingsObject, playerRadius), 0, (char*)"float : radius of the simulated circular player robot"},
    {(char*)"catch_radius", T_DOUBLE, offsetof(R2SettingsObject, catchRadius), 0, (char*)"float : maximum reach of goalkeepers, that contributes to determine catchable_distance"},
    {(char*)"catch_holding_ticks", T_INT, offsetof(R2SettingsObject, catchHoldingTicks), 0, (char*)"integer : duration in ticks of the period in which goalkeepers can hold the ball once catched"},
    {(char*)"kick_radius", T_DOUBLE, offsetof(R2SettingsObject, kickRadius), 0, (char*)"float : maximum length of a kick, that contributes to determine kickable_distance"},
    {(char*)"kickable_distance", T_DOUBLE, offsetof(R2SettingsObject, kickableDistance), 0, (char*)"float : distance to ball within which players can kick the ball. It equals (kick_radius+player_radius+ball_radius)"},
    {(char*)"catchable_distance", T_DOUBLE, offsetof(R2SettingsObject, catchableDistance), 0, (char*)"float : distance within which goalkeepers can catch the ball. It equals (catch_radius+player_radius+ball_radius)"},
    {(char*)"kickable_angle", T_DOUBLE, offsetof(R2SettingsObject, kickableAngle), 0, (char*)"float : maximum angle between player direction and player-ball direct line that permits the player to kick the ball (if the angle is greater the player will not be able to reach the ball for kicking)"},
    {(char*)"kickable_direction_angle", T_DOUBLE, offsetof(R2SettingsObject, kickableDirectionAngle), 0, (char*)"float : maximum angle between player direction and the direction of intendend kick to the ball. (The intended direction of a kick is usually expressed as in the first float value in the action tuple, after the integer value robosoc2d.ACTION_KICK). If the angle is greather than kickable_direction_angle, the player will not be able to kick the ball."},
    {(char*)"catchable_angle", T_DOUBLE, offsetof(R2SettingsObject, catchableAngle), 0, (char*)"float : maximum angle between goalkeeper direction and goalkeeper-ball direct line that permits the goalkeeper to catch the ball (if the angle is greater the goalkeeper will not be able to reach the ball to catch it)"},
    {(char*)"net_length", T_DOUBLE, offsetof(R2SettingsObject, netLength), 0, (char*)"float : the lenght, in horizontal units, of the net structure of goals."},
    {(char*)"catchable_area_length", T_DOUBLE, offsetof(R2SettingsObject, catchableAreaLength), 0, (char*)"float : the lenght, in horizontal units, of the catchable area, where goalkeepers can catch and hold the ball"},
    {(char*)"catchable_area_width", T_DOUBLE, offsetof(R2SettingsObject, catchableAreaWidth), 0, (char*)"float : the width, in horizontal units, of the catchable area, where goalkeepers can catch and hold the ball"},
    {(char*)"corner_min_distance", T_DOUBLE, offsetof(R2SettingsObject, cornerMinDistance), 0, (char*)"float : the minimum distance that opponents have to keep from ball during a corner kick. If opponents are closer, the simulator will automatically move them farther. "},
    {(char*)"throwin_min_distance", T_DOUBLE, offsetof(R2SettingsObject, throwinMinDistance), 0, (char*)"float : the minimum distance that opponents have to keep from ball during athrow-in. If opponents are closer, the simulator will automatically move them farther"},
    {(char*)"out_pitch_limit", T_DOUBLE, offsetof(R2SettingsObject, outPitchLimit), 0, (char*)"float : a distance external from pitch limits within which players can move and dash"},
    {(char*)"max_dash_power", T_DOUBLE, offsetof(R2SettingsObject, maxDashPower), 0, (char*)"float : maximum possible value for dash power (the dash power is usually expressed as second float value in the action tuple, after the integer value robosoc2d.ACTION_DASH and the first float value containing the dash direction)."},
    {(char*)"max_kick_power", T_DOUBLE, offsetof(R2SettingsObject, maxKickPower), 0, (char*)"float : maximum possible value for kick power (the kick power is usually expressed as second float value in the action tuple, after the integer value robosoc2d.ACTION_KICK and the first float value containing the kick direction)."},
    {(char*)"player_velocity_decay", T_DOUBLE, offsetof(R2SettingsObject, playerVelocityDecay), 0, (char*)"float :  decay in player velocity at each step. In common usage of the simulator you should not feel the need to either read or set this value. See the simulator source code for greater details"},
    {(char*)"ball_velocity_decay", T_DOUBLE, offsetof(R2SettingsObject, ballVelocityDecay), 0, (char*)"float : decay in ball velocity at each step. In common usage of the simulator you should not feel the need to either read or set this value. See the simulator source code for greater details"},
    {(char*)"max_player_speed", T_DOUBLE, offsetof(R2SettingsObject, maxPlayerSpeed), 0, (char*)"float : maximum speed reachable by players"},
    {(char*)"max_ball_speed", T_DOUBLE, offsetof(R2SettingsObject, maxBallSpeed), 0, (char*)"float : maximum speed reachable by ball"},
    {(char*)"catch_probability", T_DOUBLE, offsetof(R2SettingsObject,catchProbability), 0, (char*)"float : probability that goalkeepers succeed in catching the ball in case the ball is at appropriate distance and angle"},
    {(char*)"player_random_noise", T_DOUBLE, offsetof(R2SettingsObject,playerRandomNoise), 0, (char*)"float :  constant used to calculate and add some random noise to player dash and kicking power (and to ball movement). In common usage of the simulator you should not feel the need to either read or set this value.  See the simulator source code for greater details"},
    {(char*)"player_direction_noise", T_DOUBLE, offsetof(R2SettingsObject, playerDirectionNoise), 0, (char*)"float : constant used to calculate and add some random noise to player dash and kicking direction. In common usage of the simulator you should not feel the need to either read or set this value. See the simulator source code for greater details"},
    {(char*)"player_velocity_direction_mix", T_DOUBLE, offsetof(R2SettingsObject, playerVelocityDirectionMix), 0, (char*)"float : constant used in the formula to calculate player inertia. In common usage of the simulator you should not feel the need to either read or set this value.  See the simulator source code for greater details"},
    {(char*)"ball_inside_player_velocity_displace", T_DOUBLE, offsetof(R2SettingsObject, ballInsidePlayerVelocityDisplace), 0, (char*)"float : constant used in the formula to calculate player inertia. In common usage of the simulator you should not feel the need to either read or set this value.  See the simulator source code for greater details"},
    {(char*)"after_catch_distance", T_DOUBLE, offsetof(R2SettingsObject, afterCatchDistance), 0, (char*)"float : constant used to calculate the position of the ball when the goalkeeper that caught the ball releases the ball after the catch time has terminated. The ball will be approximately at a distance equal to (player_radius+ball_radius+after_catch_distance) from the position of the goalkeeper, along the goalkeeper direction vector"},
    {NULL}  /* Sentinel */
};

struct R2PitchObject {
    PyObject_HEAD
    double x1;
    double x2;
    double y1;
    double y2;
    double xGoal1;
    double xGoal2;
    double yGoal1;
    double yGoal2;
    double areaLx; 
    double areaRx;
    double areaUy;
    double areaDy;
    double goalKickLx;
    double goalKickRx;
    double goalKickUy;
    double goalKickDy;
    double pole1x;
    double pole1y;
    double pole2x;
    double pole2y; 
    double pole3x;
    double pole3y;
    double pole4x;
    double pole4y;
    double border_up;
    double border_down;
    double border_left;
    double border_right;
};

static PyObject *R2PitchObject_repr(R2PitchObject *obj){
    std::stringstream buffer;
    buffer << "{" 
        << "'x1': " << obj->x1 << ", "
        << "'x2': " << obj->x2 << ", "
        << "'y1': " << obj->y1 << ", "
        << "'y2': " << obj->y2 << ", "
        << "'x_goal1': " << obj->xGoal1 << ", "
        << "'x_goal2': " << obj->xGoal2 << ", "
        << "'y_goal1': " << obj->yGoal1 << ", "
        << "'y_goal2': " << obj->yGoal2 << ", "
        << "'area_lx': " << obj->areaLx << ", "
        << "'area_rx': " << obj->areaRx << ", "
        << "'area_uy': " << obj->areaUy << ", "
        << "'area_dy': " << obj->areaDy << ", "
        << "'goal_kick_lx': " << obj->goalKickLx << ", "
        << "'goal_kick_rx': " << obj->goalKickRx << ", "
        << "'goal_kick_uy': " << obj->goalKickUy << ", "
        << "'goal_kick_dy': " << obj->goalKickDy << ", "
        << "'pole1x': " << obj->pole1x << ", "
        << "'pole1y': " << obj->pole1y << ", "
        << "'pole2x': " << obj->pole2x << ", "
        << "'pole2y': " << obj->pole2y << ", "
        << "'pole3x': " << obj->pole3x << ", "
        << "'pole3y': " << obj->pole3y << ", "
        << "'pole4x': " << obj->pole4x << ", "
        << "'pole4y': " << obj->pole4y << ", "
        << "'border_up': " << obj->border_up << ", "
        << "'border_down': " << obj->border_down << ", "
        << "'border_left': " << obj->border_left << ", "
        << "'border_right': " << obj->border_right 
        << "}" ;
    return PyUnicode_FromString(buffer.str().c_str());
}

static void
R2PitchType_dealloc(R2PitchObject *obj)
{
    Py_TYPE(obj)->tp_free((PyObject *) obj);
}

void fillR2PitchObject(R2PitchObject& target, const R2Pitch& source){
    target.x1 = source.x1;
    target.x2 = source.x2;
    target.y1 = source.y1;
    target.y2 = source.y2;
    target.xGoal1 = source.xGoal1;
    target.xGoal2 = source.xGoal2;
    target.yGoal1 = source.yGoal1;
    target.yGoal2 = source.yGoal2;
    target.areaLx = source.areaLx; 
    target.areaRx = source.areaRx;
    target.areaUy = source.areaUy;
    target.areaDy = source.areaDy;
    target.goalKickLx = source.goalKickLx;
    target.goalKickRx = source.goalKickRx;
    target.goalKickUy = source.goalKickUy;
    target.goalKickDy = source.goalKickDy;
    target.pole1x = source.poles[0].x;
    target.pole1y = source.poles[0].y;
    target.pole2x = source.poles[1].x;
    target.pole2y = source.poles[1].y; 
    target.pole3x = source.poles[2].x;
    target.pole3y = source.poles[2].y;
    target.pole4x = source.poles[3].x;
    target.pole4y = source.poles[3].y;
    target.border_up = source.border_up;
    target.border_down = source.border_down;
    target.border_left = source.border_left;
    target.border_right = source.border_right;
}

const char R2Pitch_doc[] = "Object containing information about the game pitch.\n\n\
The method copy() returns a binary copy of this object.\n\
Converting an object to string and then evaluating that string with eval() will result in a dictionary containing all the fields of the object.\n\
E.g. my_dict=eval(str(my_pitch))\n\n";

static PyTypeObject R2PitchType =
{
    PyVarObject_HEAD_INIT(NULL, 0)
};

static PyObject *
R2Pitch_copy(R2PitchObject *self, PyObject *Py_UNUSED(ignored))
{
    R2PitchObject* target=PyObject_New(R2PitchObject, &R2PitchType);
    if(target==NULL)
    {
	    PyErr_SetString(R2Error, "unable to create pitch object");
        return NULL;
    }

    target->x1 = self->x1;
    target->x2 = self->x2;
    target->y1 = self->y1;
    target->y2 = self->y2;
    target->xGoal1 = self->xGoal1;
    target->xGoal2 = self->xGoal2;
    target->yGoal1 = self->yGoal1;
    target->yGoal2 = self->yGoal2;
    target->areaLx = self->areaLx; 
    target->areaRx = self->areaRx;
    target->areaUy = self->areaUy;
    target->areaDy = self->areaDy;
    target->goalKickLx = self->goalKickLx;
    target->goalKickRx = self->goalKickRx;
    target->goalKickUy = self->goalKickUy;
    target->goalKickDy = self->goalKickDy;
    target->pole1x = self->pole1x;
    target->pole1y = self->pole1y;
    target->pole2x = self->pole2x;
    target->pole2y = self->pole2y;
    target->pole3x = self->pole3x;
    target->pole3y = self->pole3y;
    target->pole4x = self->pole4x;
    target->pole4y = self->pole4y;
    target->border_up = self->border_up;
    target->border_down = self->border_down;
    target->border_left = self->border_left;
    target->border_right = self->border_right;

    return (PyObject*)target;
}

static PyMethodDef R2Pitch_methods[] = {
    {"copy", (PyCFunction) R2Pitch_copy, METH_NOARGS,
     "Builds and returns a copy"
    },
    {NULL}  /* Sentinel */
};

static PyMemberDef R2Pitch_members[] = {
    {(char*)"x1", T_DOUBLE, offsetof(R2PitchObject, x1), 0, (char*)"float : horizontal coordinate of right pitch boundary (usually a positive number)"},
    {(char*)"x2", T_DOUBLE, offsetof(R2PitchObject, x2), 0, (char*)"float : horizontal coordinate of left pitch boundary (usually a negative number = -x1)"},
    {(char*)"y1", T_DOUBLE, offsetof(R2PitchObject, y1), 0, (char*)"float : vertical coordinate of upper pitch boundary (usually a postiive number)"},
    {(char*)"y2", T_DOUBLE, offsetof(R2PitchObject, y2), 0, (char*)"float : vertical coordinate of lower pitch boundary (usually a negative number = -y1)"},    
    {(char*)"x_goal1", T_DOUBLE, offsetof(R2PitchObject, xGoal1), 0, (char*)"float : horizontal coordinate of the right end of the right goal considering the net. The right goal begins at coordinate x1 (at the right pitch boundary, where if the ball surpass the vertical line you score a goal) and it ends at coordinate x_goal1 "},
    {(char*)"x_goal2", T_DOUBLE, offsetof(R2PitchObject, xGoal2), 0, (char*)"float : horizontal coordinate of the left end of the left goal considering the net. The lect goal begins at coordinate x2 (at the left pitch boundary, where if the ball surpass the vertical line you score a goal) and it ends at coordinate x_goal2"},
    {(char*)"y_goal1", T_DOUBLE, offsetof(R2PitchObject, yGoal1), 0, (char*)"float : vertical coordinate of the upper end of the goals, where you have upper poles"},
    {(char*)"y_goal2", T_DOUBLE, offsetof(R2PitchObject, yGoal2), 0, (char*)"float : vertical coordinate of the lower end of the goals, where you have lower poles"},
    {(char*)"area_lx", T_DOUBLE, offsetof(R2PitchObject, areaLx), 0, (char*)"float : right horizontal boundary of the left area (the left area has left horizontal boundary in x2)"},
    {(char*)"area_rx", T_DOUBLE, offsetof(R2PitchObject, areaRx), 0, (char*)"float : left horizontal boundary of the right area (the right area has right horizontal boundary in x1)"},
    {(char*)"area_uy", T_DOUBLE, offsetof(R2PitchObject, areaUy), 0, (char*)"float : upper vertical boundary of areas"},
    {(char*)"area_dy", T_DOUBLE, offsetof(R2PitchObject, areaDy), 0, (char*)"float : lower (down) vertical boundary of areas"},
    {(char*)"goal_kick_lx", T_DOUBLE, offsetof(R2PitchObject, goalKickLx), 0, (char*)"float : horizontal coordinate of left goal kicks position"},
    {(char*)"goal_kick_rx", T_DOUBLE, offsetof(R2PitchObject, goalKickRx), 0, (char*)"float : horizontal coordinate of right goal kicks position"},
    {(char*)"goal_kick_uy", T_DOUBLE, offsetof(R2PitchObject, goalKickUy), 0, (char*)"float : vertical coordinate of upper goal kicks position"},
    {(char*)"goal_kick_dy", T_DOUBLE, offsetof(R2PitchObject, goalKickDy), 0, (char*)"float : vertical coordinate of lower (down) goal kicks position"},
    {(char*)"pole1x", T_DOUBLE, offsetof(R2PitchObject, pole1x), 0, (char*)"float : horizontal coordinate of left upper pole"},
    {(char*)"pole1y", T_DOUBLE, offsetof(R2PitchObject, pole1y), 0, (char*)"float : vertical coordinate of left upper pole"},
    {(char*)"pole2x", T_DOUBLE, offsetof(R2PitchObject, pole2x), 0, (char*)"float : horizontal coordinate of left lower pole"},
    {(char*)"pole2y", T_DOUBLE, offsetof(R2PitchObject, pole2y), 0, (char*)"float : vertical coordinate of the left lower pole"}, 
    {(char*)"pole3x", T_DOUBLE, offsetof(R2PitchObject, pole3x), 0, (char*)"float : horizontal coordinate of right upper pole"},
    {(char*)"pole3y", T_DOUBLE, offsetof(R2PitchObject, pole3y), 0, (char*)"float : vertical coordinate of right upper pole"},
    {(char*)"pole4x", T_DOUBLE, offsetof(R2PitchObject, pole4x), 0, (char*)"float : horizontal coordinate of right lower pole"},
    {(char*)"pole4y", T_DOUBLE, offsetof(R2PitchObject, pole4y), 0, (char*)"float : vertical coordinate of right lower pole"},
    {(char*)"border_up", T_DOUBLE, offsetof(R2PitchObject, border_up), 0, (char*)"float : vertical coordinate of the extreme upper end of the playfield, the more external upper coordinate where a player can be (it's outside the pitch"},
    {(char*)"border_down", T_DOUBLE, offsetof(R2PitchObject, border_down), 0, (char*)"float : vertical coordinate of the extreme lower end of the playfield, the more external lower coordinate where a player can be (it's outside the pitch"},
    {(char*)"border_left", T_DOUBLE, offsetof(R2PitchObject, border_left), 0, (char*)"float : horizontal coordinate of the extreme left end of the playfield, the more external left coordinate where a player can be (it's outside the pitch)"},
    {(char*)"border_right", T_DOUBLE, offsetof(R2PitchObject, border_right), 0, (char*)"float : horizontal coordinate of the extreme right end of the playfield, the more external right coordinate where a player can be (it's outside the pitch)"},
    {NULL}  /* Sentinel */
};

struct R2EnvironmentObject {
    PyObject_HEAD
    int tick;    
    int score1;
    int score2;
    int state;
    double ballX;
    double ballY;
    double ballVelocityX;
    double ballVelocityY;
    bool lastTouchedTeam2;  
    double startingTeamMaxRange;   
    int ballCatched;        
    bool ballCatchedTeam2;
    bool halftimePassed;
    int nPlayers1;
    int nPlayers2;
};

static PyObject *R2EnvironmentObject_repr(R2EnvironmentObject *obj){
    std::stringstream buffer;
    buffer << "{" 
        << "'tick': " << obj->tick << ", "
        << "'score1': " << obj->score1 << ", "
        << "'score2': " << obj->score2 << ", "
        << "'state': " << obj->state << ", "
        << "'ball_x': " << obj->ballX << ", "
        << "'ball_y': " << obj->ballY << ", "
        << "'ball_velocity_x': " << obj->ballVelocityX << ", "
        << "'ball_velocity_y': " << obj->ballVelocityY << ", "
        << "'last_touched_team2': " << (obj->lastTouchedTeam2 ? "True" : "False") << ", "
        << "'starting_team_max_range': " << obj->startingTeamMaxRange << ", "
        << "'ball_catched': " << obj->ballCatched << ", "
        << "'ball_catched_team2': " << (obj-> ballCatchedTeam2 ? "True" : "False") << ", "
        << "'halftime_passed': " << (obj-> halftimePassed ? "True" : "False") << ", "
        << "'n_players1': " << obj->nPlayers1 << ", "
        << "'n_players2': " << obj->nPlayers2 
        << "}" ;
    return PyUnicode_FromString(buffer.str().c_str());
}

static void
R2EnvironmentType_dealloc(R2EnvironmentObject *obj)
{
    Py_TYPE(obj)->tp_free((PyObject *) obj);
}


void fillR2EnvironmentObject(R2EnvironmentObject& target, const R2Environment& source){
    target.tick = source.tick;    
    target.score1 = source.score1;
    target.score2 = source.score2;
    target.state = static_cast<int>(source.state);
    target.ballX = source.ball.pos.x;
    target.ballY = source.ball.pos.y;
    target.ballVelocityX = source.ball.velocity.x;
    target.ballVelocityY = source.ball.velocity.y;
    target.lastTouchedTeam2 = source.lastTouchedTeam2;  
    target.startingTeamMaxRange = source.startingTeamMaxRange;   
    target.ballCatched = source.ballCatched;        
    target.ballCatchedTeam2 = source.ballCatchedTeam2;  
    target.halftimePassed = source.halftimePassed;  
    target.nPlayers1 = source.teams[0].size();
    target.nPlayers2 = source.teams[1].size();
}

const char R2Environment_doc[] = "Object containing the state of the simulation environment.\n\n\
The method copy() returns a binary copy of this object.\n\
Converting an object to string and then evaluating that string with eval() will result in a dictionary containing all the fields of the object.\n\
E.g. my_dict=eval(str(my_environment))\n\n";

static PyTypeObject R2EnvironmentType =
{
    PyVarObject_HEAD_INIT(NULL, 0)
};

static PyObject *
R2Environment_copy(R2EnvironmentObject *self, PyObject *Py_UNUSED(ignored))
{
    R2EnvironmentObject* target=PyObject_New(R2EnvironmentObject, &R2EnvironmentType);
    if(target==NULL)
    {
	    PyErr_SetString(R2Error, "unable to create environment object");
        return NULL;
    }

    target->tick = self->tick;    
    target->score1 = self->score1;
    target->score2 = self->score2;
    target->state = self->state;
    target->ballX = self->ballX;
    target->ballY = self->ballY;
    target->ballVelocityX = self->ballVelocityX;
    target->ballVelocityY = self->ballVelocityY;
    target->lastTouchedTeam2 = self->lastTouchedTeam2;  
    target->startingTeamMaxRange = self->startingTeamMaxRange;   
    target->ballCatched = self->ballCatched;        
    target->ballCatchedTeam2 = self->ballCatchedTeam2;  
    target->halftimePassed = self->halftimePassed;  
    target->nPlayers1 = self->nPlayers1;
    target->nPlayers2 = self->nPlayers2;

    return (PyObject*)target;
}

static PyMethodDef R2Environment_methods[] = {
    {"copy", (PyCFunction) R2Environment_copy, METH_NOARGS,
     "Builds and returns a copy"
    },
    {NULL}  /* Sentinel */
};

static PyMemberDef R2Environment_members[] = {
    {(char*)"tick", T_INT, offsetof(R2EnvironmentObject, tick), 0, (char*)"integer : the current tick (a tick is a timestep of the game)"},
    {(char*)"score1", T_INT, offsetof(R2EnvironmentObject, score1), 0, (char*)"integer : the score by first team"},
    {(char*)"score2", T_INT, offsetof(R2EnvironmentObject, score2), 0, (char*)"integer : the score by second team"},
    {(char*)"state", T_INT, offsetof(R2EnvironmentObject, state), 0, (char*)"integer : an integer representing the state of the game, such as robosoc2d.STATE_PLAY . Possible values, all inside the robosoc2d module, are: STATE_INACTIVE, STATE_READY (unused), STATE_KICKOFF1, STATE_KICKOFF2, STATE_PLAY, STATE_STOPPED (unused), STATE_GOALKICK1UP, STATE_GOALKICK1DOWN, STATE_GOALKICK2UP, STATE_GOALKICK2DOWN, STATE_CORNER1UP, STATE_CORNER1DOWN, STATE_CORNER2UP, STATE_CORNER2DOWN, STATE_THROWIN1, STATE_THROWIN2, STATE_PAUSED (unused), STATE_HALFTIME, STATE_GOAL1, STATE_GOAL2, STATE_ENDED"},
    {(char*)"ball_x", T_DOUBLE, offsetof(R2EnvironmentObject, ballX), 0, (char*)"float : horizontal coordinate of ball position"},   
    {(char*)"ball_y", T_DOUBLE, offsetof(R2EnvironmentObject, ballY), 0, (char*)"float : vertical coordinate of ball position"},   
    {(char*)"ball_velocity_x", T_DOUBLE, offsetof(R2EnvironmentObject, ballVelocityX), 0, (char*)"float : horizontal component of ball's velocity vector"},   
    {(char*)"ball_velocity_y", T_DOUBLE, offsetof(R2EnvironmentObject, ballVelocityY), 0, (char*)"float : vertical component of ball's velocity vector"},   
    {(char*)"last_touched_team2", T_BOOL, offsetof(R2EnvironmentObject, lastTouchedTeam2), 0, (char*)"boolean : set to True if last player to touch or kick the ball was belonging to second team, or set to False if belonging to first team"},  
    {(char*)"starting_team_max_range", T_DOUBLE, offsetof(R2EnvironmentObject, startingTeamMaxRange), 0, (char*)"float : maximum range of movement for players of the non-kicking team in case of throw-in, corner kick, or goal-kick"},     
    {(char*)"ball_catched", T_INT, offsetof(R2EnvironmentObject, ballCatched), 0, (char*)"integer : number of ticks still available to goalkeeper to hold the ball"},        
    {(char*)"ball_catched_team2", T_BOOL, offsetof(R2EnvironmentObject, ballCatchedTeam2), 0, (char*)"boolean : in case a goalkeeper has catched the ball this attribute is set to True if it's the second team's goalkeeper, or set to False if it's the first team's goalkeeper"},  
    {(char*)"halftime_passed", T_BOOL, offsetof(R2EnvironmentObject, halftimePassed), 0, (char*)"boolean : this is set to True after the second half of the match has begun, otherwise it is set to False"},  
    {(char*)"n_players1", T_INT, offsetof(R2EnvironmentObject, nPlayers1), 0, (char*)"integer : number of players of first team"},
    {(char*)"n_players2", T_INT, offsetof(R2EnvironmentObject, nPlayers2), 0, (char*)"integer : number of players of second team"},
    {NULL}  /* Sentinel */
};

struct R2PlayerInfoObject {
    PyObject_HEAD
    double x;
    double y;
    double velocityX;
    double velocityY;
    double direction; 
    bool acted; 
};

static PyObject *R2PlayerInfoObject_repr(R2PlayerInfoObject *obj){
    std::stringstream buffer;
    buffer << "{" 
        << "'x': " << obj->x << ", "
        << "'y': " << obj->y << ", "
        << "'velocity_x': " << obj->velocityX << ", "
        << "'velocity_y': " << obj->velocityY << ", "
        << "'direction': " << obj->direction << ", "
        << "'acted': " << (obj->acted ? "True" : "False")
        << "}" ;
    return PyUnicode_FromString(buffer.str().c_str());
}

static void
R2PlayerInfoType_dealloc(R2PlayerInfoObject *obj)
{
    Py_TYPE(obj)->tp_free((PyObject *) obj);
}

void fillR2PlayerInfoObject(R2PlayerInfoObject& target, const R2PlayerInfo& source){
    target.x = source.pos.x;
    target.y = source.pos.y;
    target.velocityX = source.velocity.x;
    target.velocityY = source.velocity.y;
    target.direction = source.direction; 
    target.acted = source.acted; 
}

void setR2PlayerInfo(R2PlayerInfo& target, const R2PlayerInfoObject& source){
    target.pos.x = source.x;
    target.pos.y = source.y;
    target.velocity.x = source.velocityX;
    target.velocity.y = source.velocityY;
    target.direction = source.direction; 
    target.acted = source.acted; 
}

const char R2PlayerInfo_doc[] = "Object representing information about a player.\n\n\
The method copy() returns a binary copy of this object.\n\
Converting an object to string and then evaluating that string with eval() will result in a dictionary containing all the fields of the object.\n\
E.g. my_dict=eval(str(my_player_info))\n\n";

static PyTypeObject R2PlayerInfoType =
{
    PyVarObject_HEAD_INIT(NULL, 0)
};

static PyObject *
R2PlayerInfo_copy(R2PlayerInfoObject *self, PyObject *Py_UNUSED(ignored))
{
    R2PlayerInfoObject* target=PyObject_New(R2PlayerInfoObject, &R2PlayerInfoType);
    if(target==NULL)
    {
	    PyErr_SetString(R2Error, "unable to create environment object");
        return NULL;
    }

    target->x = self->x;
    target->y = self->y;
    target->velocityX = self->velocityX;
    target->velocityY = self->velocityY;
    target->direction = self->direction; 
    target->acted = self->acted; 

    return (PyObject*)target;
}

static PyMethodDef R2PlayerInfo_methods[] = {
    {"copy", (PyCFunction) R2PlayerInfo_copy, METH_NOARGS,
     "Builds and returns a copy"
    },
    {NULL}  /* Sentinel */
};

static PyMemberDef R2PlayerInfo_members[] = {
    {(char*)"x", T_DOUBLE, offsetof(R2PlayerInfoObject, x), 0, (char*)"float : horizontal coordinate of player position"},
    {(char*)"y", T_DOUBLE, offsetof(R2PlayerInfoObject, y), 0, (char*)"float : vertical coordinate of player position"},
    {(char*)"velocity_x", T_DOUBLE, offsetof(R2PlayerInfoObject, velocityX), 0, (char*)"float : horizontal component of player's velocity vector"},
    {(char*)"velocity_y", T_DOUBLE, offsetof(R2PlayerInfoObject, velocityY), 0, (char*)"float : vertical component of player's velocity vector"},
    {(char*)"direction", T_DOUBLE, offsetof(R2PlayerInfoObject, direction), 0, (char*)"float : direction of the player, expressed in radians"}, 
    {(char*)"acted", T_BOOL, offsetof(R2PlayerInfoObject, acted), 0, (char*)"float : set to True if player has alraedy acted in current Tick"}, 
    {NULL}  /* Sentinel */
};

// returns a tuple containing environment, pitch, settings, team1 info, team2 info
// the reference is stolen by caller, that will own it
static PyObject* pythonizeGameState(const R2GameState& gameState) {
    R2EnvironmentObject* env=PyObject_New(R2EnvironmentObject, &R2EnvironmentType);
    if(env==NULL)
        return NULL;
    fillR2EnvironmentObject(*env, gameState.env);

    R2PitchObject* pitch=PyObject_New(R2PitchObject, &R2PitchType);
    if(pitch==NULL)
    {
        Py_DECREF(env);
        return NULL;
    }
    fillR2PitchObject(*pitch, gameState.pitch);

    R2SettingsObject* sett=PyObject_New(R2SettingsObject, &R2SettingsType);
    if(sett==NULL){
        Py_DECREF(env);
        Py_DECREF(pitch);
        return NULL;
    }
    fillR2SettingsObject(*sett, gameState.sett);

    PyObject *players1 = PyTuple_New(gameState.env.teams[0].size());
    if(players1==NULL){
        Py_DECREF(env);
        Py_DECREF(pitch);
        Py_DECREF(sett);
        return NULL;
    }

    for(unsigned int i=0; i<gameState.env.teams[0].size(); i++){
        R2PlayerInfoObject* playerInfo=PyObject_New(R2PlayerInfoObject, &R2PlayerInfoType);
        if(sett==NULL){
            Py_DECREF(env);
            Py_DECREF(pitch);
            Py_DECREF(sett);
            Py_DECREF(players1);
            return NULL;
        }
        fillR2PlayerInfoObject(*playerInfo, gameState.env.teams[0][i]);
        PyTuple_SetItem(players1, i, (PyObject*)playerInfo);
    }

    PyObject *players2 = PyTuple_New(gameState.env.teams[1].size());
    if(players1==NULL){
        Py_DECREF(env);
        Py_DECREF(pitch);
        Py_DECREF(sett);
        Py_DECREF(players1);
        return NULL;
    }

    for(unsigned int i=0; i<gameState.env.teams[1].size(); i++){
        R2PlayerInfoObject* playerInfo=PyObject_New(R2PlayerInfoObject, &R2PlayerInfoType);
        if(sett==NULL){
            Py_DECREF(env);
            Py_DECREF(pitch);
            Py_DECREF(sett);
            Py_DECREF(players1);
            Py_DECREF(players2);
            return NULL;
        }
        fillR2PlayerInfoObject(*playerInfo, gameState.env.teams[1][i]);
        PyTuple_SetItem(players2, i, (PyObject*)playerInfo);
    }

    PyObject *pyGameState = PyTuple_New(5);
    if(pyGameState==NULL){
        Py_DECREF(env);
        Py_DECREF(pitch);
        Py_DECREF(sett);
        Py_DECREF(players1);
        Py_DECREF(players2);
        return NULL;
    }
    PyTuple_SetItem(pyGameState, 0, (PyObject*)env);
    PyTuple_SetItem(pyGameState, 1, (PyObject*)pitch);
    PyTuple_SetItem(pyGameState, 2, (PyObject*)sett);
    PyTuple_SetItem(pyGameState, 3, (PyObject*)players1);
    PyTuple_SetItem(pyGameState, 4, (PyObject*)players2);
    return pyGameState;
}

static PyObject *stepMethodName;
class PythonPlayer : public R2Player {
private:
    PyObject *pythonPlayerObject;
public:
    PythonPlayer(PyObject *player);
    ~PythonPlayer(); 

    virtual R2Action step(const R2GameState gameState) override;
};

R2Action PythonPlayer::step(const R2GameState gameState) {
    if(pythonPlayerObject!=NULL){
        PyObject *args =pythonizeGameState(gameState);
        if(args==NULL){
            return R2Action();
        }

        // python player step() method
        PyObject *result = PyObject_CallMethodObjArgs(pythonPlayerObject, stepMethodName, PyTuple_GetItem(args,0), PyTuple_GetItem(args,1),
                                                        PyTuple_GetItem(args,2), PyTuple_GetItem(args,3), PyTuple_GetItem(args,4), NULL);  // NOTE: final NULL is necessary to signal NULL-termination of arguments!
        Py_DECREF(args);
                
        // NOW ELABORATE result
        if(result == NULL){
            return R2Action();
        }

        if(!PySequence_Check(result)){
            Py_DECREF(result);  //check
            return R2Action();
        }

        int len=(int)PySequence_Length(result);
        if(len != 4){
            Py_DECREF(result);  //check
            return R2Action();
        }

        R2Action playerAction;

        PyObject *po = PySequence_GetItem(result, 0);
        if (po == NULL){ 
            Py_DECREF(result);  //check
            return R2Action();
        }
        if(!PyLong_Check(po)){
            Py_DECREF(po);
            Py_DECREF(result);  //check
            return R2Action();
        }
        playerAction.action=static_cast<R2ActionType>((int)PyLong_AsLong(po));
        Py_DECREF(po);

        for(int i=0; i<3; i++){
            po = PySequence_GetItem(result, i+1);
            if (po == NULL){ 
                return R2Action();
            }
            if(!PyFloat_Check(po)){
                Py_DECREF(po);
                return R2Action();
            }
            playerAction.data[i]=PyFloat_AsDouble(po);
            Py_DECREF(po);
        }
        Py_DECREF(result);  //check

        return playerAction;    // only point in which the legit action from the python player is built and used
    }
    return R2Action();
}

PythonPlayer::PythonPlayer(PyObject *player){
    pythonPlayerObject=player;
    Py_XINCREF(pythonPlayerObject);
}

PythonPlayer::~PythonPlayer(){
    if(isPythonActive)
        Py_XDECREF(pythonPlayerObject);
} 

static int serialSimulations=1;

static auto simulations = std::unordered_map<int, std::shared_ptr<R2Simulator>> {};

static PyObject *robosoc2d_getVersion(PyObject *self, PyObject *args){
    return PyUnicode_FromString(GetR2SVersion());
}

static PyObject *robosoc2d_getDefaultSettings(PyObject *self, PyObject *args){
    R2SettingsObject* sett=PyObject_New(R2SettingsObject, &R2SettingsType);
    if(sett==NULL){
        PyErr_SetString(R2Error, "unable to create settings object");
        return NULL;
    }
    fillR2SettingsObject(*sett, R2EnvSettings());
    return (PyObject*)sett;
}

static PyObject *robosoc2d_getSeedByCurrentTime(PyObject *self, PyObject *args){
    int randomSeed=(int)createChronoRandomSeed();
    return PyLong_FromLong(randomSeed);
}

// return the handle of the simulator, 0 if failed
static int createSimulator(std::vector<std::shared_ptr<R2Player>> team1, std::vector<std::shared_ptr<R2Player>> team2,
     std::string team1name, std::string team2name,
     unsigned int random_seed= createChronoRandomSeed(),
      R2EnvSettings settings= R2EnvSettings()){

    int key=serialSimulations;
    serialSimulations++;

    auto[iterator, success] = simulations.emplace(
                        std::make_pair(key, std::make_shared<R2Simulator>(team1, team2, team1name, team2name, random_seed, settings) )
    );
    
    if(success)
        return key;
    
    return 0;
}

// Sequence of robosoc2d.player: provided team1 with user's logic
// Sequence of robosoc2d.player: provided team2 with user's logic
// integer (optional): random seed to be used by the simulation random number generators
// reference to a robosoc2d.settings object (optional): settings to be used to build the simulation
static PyObject *robosoc2d_buildSimulator(PyObject *self, PyObject *args, PyObject *keywds) {
    static char *keywords[] = {(char *)"team1", (char *)"team2", (char *)"team1name", (char *)"team2name", (char *)"random_seed", (char *)"game_settings", NULL};
    int handle=0;
    PyObject *pTeamSequence[2]{NULL,NULL};
    const char* teamNames[2]{NULL,NULL};
    int randomSeed=(int)createChronoRandomSeed();
    PyObject *pObj=NULL;
    R2SettingsObject *pSettings;
    R2EnvSettings cSettings;

    if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|ssiO", keywords, &pTeamSequence[0], &pTeamSequence[1], &teamNames[0], &teamNames[1], &randomSeed, &pObj)){
        PyErr_SetString(PyExc_TypeError, "wrong parameters");
        return NULL;
    }

    std::vector<std::shared_ptr<R2Player>> team[2];

    for(int t=0; t<=1; t++){
        if(!PySequence_Check(pTeamSequence[t])){
            PyErr_SetString(PyExc_TypeError, "first and second arguments must be a sequence of players");
            return NULL;
        }

        int len=(int)PySequence_Length(pTeamSequence[t]);

        for (int i = 0; i < len; i++) { 
            PyObject *player = PySequence_GetItem(pTeamSequence[t], i);
            if (player == NULL){ // some failure
                PyErr_SetString(PyExc_TypeError, "unable to get player object from sequence");
                return NULL;
            }

            if(! PyObject_HasAttr(player, stepMethodName)){
                PyErr_SetString(PyExc_TypeError, "players must implement method function 'step' ");
                Py_DECREF(player);
                return NULL;
            }
            
            team[t].push_back(std::static_pointer_cast<R2Player>(std::make_shared<PythonPlayer>(player)));
            Py_DECREF(player);
        }
    }

    std::string name1((teamNames[0]!=NULL)? teamNames[0] : defaultPythonTeam1Name);
    std::string name2((teamNames[1]!=NULL)? teamNames[1] : defaultPythonTeam2Name);

    if(pObj != NULL){
        if(pObj->ob_type != &R2SettingsType){
            PyErr_SetString(PyExc_TypeError, "wrong type for settings parameter");
            return NULL;
        }
        pSettings=(R2SettingsObject*)pObj;
        fillR2Settings(cSettings, *pSettings);
        handle= createSimulator(team[0], team[1], name1, name2, (unsigned int)randomSeed, cSettings);
    }
    else{  
        handle= createSimulator(team[0], team[1], name1, name2, (unsigned int)randomSeed);
    }

    if (handle!=0)
        return PyLong_FromLong(handle);

    PyErr_SetString(R2Error, "impossible to create simulator");
    return NULL;
}

// Sequence of robosoc2d.player: provided team1 with user's logic
// integer: number of simpleplayer to add as first team1 players (the first SimplePlayer will play as goalkeeper)
// Sequence of robosoc2d.player: provided team2 with user's logic
// integer: number of simpleplayer to add as first team2 players (the first SimplePlayer will play as goalkeeper)
// integer (optional): random seed to be used by the simulation random number generators
// reference to a robosoc2d.settings object (optional): settings to be used to build the simulation
static PyObject *robosoc2d_buildSimplePlayerSimulator(PyObject *self, PyObject *args, PyObject *keywds) {
    static char *keywords[] = {(char *)"team1", (char *)"how_manysimpleplayers_team1", (char *)"team2", (char *)"how_manysimpleplayers_team1", (char *)"team1name", (char *)"team2name", (char *)"random_seed", (char *)"game_settings", NULL};
    int handle=0;
    PyObject *pTeamSequence[2]{NULL,NULL};
    int createSimplePlayers[2]{0,0};
    const char* teamNames[2]{NULL,NULL};
    int randomSeed=(int)createChronoRandomSeed();
    PyObject *pObj=NULL;
    R2SettingsObject *pSettings;
    R2EnvSettings cSettings;

    if (!PyArg_ParseTupleAndKeywords(args, keywds, "OiOi|ssiO", keywords, &pTeamSequence[0], &createSimplePlayers[0], &pTeamSequence[1], &createSimplePlayers[1],  &teamNames[0], &teamNames[1], &randomSeed, &pObj)){
        PyErr_SetString(PyExc_TypeError, "wrong parameters");
        return NULL;
    }

    std::vector<std::shared_ptr<R2Player>> team[2];

    for(int t=0; t<=1; t++){
        for(int i=0; i<createSimplePlayers[t]; i++){
            team[t].push_back(std::static_pointer_cast<R2Player>(std::make_shared<SimplePlayer>(i, t)));
        }
    
        if(!PySequence_Check(pTeamSequence[t])){
            PyErr_SetString(PyExc_TypeError, "second argument must be a sequence of players");
            return NULL;
        }

        int len=(int)PySequence_Length(pTeamSequence[t]);

        for (int i = 0; i < len; i++) { 
            PyObject *player = PySequence_GetItem(pTeamSequence[t], i);
            if (player == NULL){ // some failure
                PyErr_SetString(PyExc_TypeError, "unable to get player object from sequence");
                return NULL;
            }

            if(! PyObject_HasAttr(player, stepMethodName)){
                PyErr_SetString(PyExc_TypeError, "players must implement method function 'step' ");
                Py_DECREF(player);
                return NULL;
            }
            
            team[t].push_back(std::static_pointer_cast<R2Player>(std::make_shared<PythonPlayer>(player)));
            Py_DECREF(player);
        }
    }

    std::string name1((teamNames[0]!=NULL)? teamNames[0] : defaultPythonTeam1Name);
    std::string name2((teamNames[1]!=NULL)? teamNames[1] : defaultPythonTeam2Name);

    if(pObj != NULL){
        if(pObj->ob_type != &R2SettingsType){
            PyErr_SetString(PyExc_TypeError, "wrong type for settings parameter");
            return NULL;
        }
        pSettings=(R2SettingsObject*)pObj;
        fillR2Settings(cSettings, *pSettings);
        handle= createSimulator(team[0], team[1], name1, name2, (unsigned int)randomSeed, cSettings);
    }
    else{  
        handle= createSimulator(team[0], team[1], name1, name2, (unsigned int)randomSeed);
    }

    if (handle!=0)
        return PyLong_FromLong(handle);

    PyErr_SetString(R2Error, "impossible to create simulator");
    return NULL;
}


static PyObject *robosoc2d_simulatorStepIfPlaying(PyObject *self, PyObject *args, PyObject *keywds){
    static char *keywords[] = {(char *)"handle", NULL};
    int handle;
    if (!PyArg_ParseTupleAndKeywords(args, keywds, "i", keywords, &handle)){
        PyErr_SetString(PyExc_TypeError, "parameter must be an integer");
        return NULL;
    }

    if(simulations.count(handle) == 0){
        PyErr_SetString(R2Error, "simulation handle not existing");
        return NULL;
    }

    return PyBool_FromLong(long(simulations[handle]->stepIfPlaying()));
}

static PyObject *robosoc2d_simulatorDelete(PyObject *self, PyObject *args, PyObject *keywds){
    static char *keywords[] = {(char *)"handle", NULL};
    int handle;
    if (!PyArg_ParseTupleAndKeywords(args, keywds, "i", keywords, &handle)){
        PyErr_SetString(PyExc_TypeError, "parameter must be an integer");
        return NULL;
    }

    if(simulations.count(handle) == 0){
        PyErr_SetString(R2Error, "simulation handle not existing");
        return NULL;
    }

    return PyBool_FromLong(simulations.erase(handle));
}

static PyObject *robosoc2d_simulatorPlayGame(PyObject *self, PyObject *args, PyObject *keywds){
    static char *keywords[] = {(char *)"handle", NULL};
    int handle;
    if (!PyArg_ParseTupleAndKeywords(args, keywds, "i", keywords, &handle)){
        PyErr_SetString(PyExc_TypeError, "parameter must be an integer");
        return NULL;
    }

    if(simulations.count(handle) == 0){
        PyErr_SetString(R2Error, "simulation handle not existing");
        return NULL;
    }

    while(simulations[handle]->stepIfPlaying()){
        //R2Environment env=simulations[handle]->getGameState().env;

        DEBUG_OUT(simulations[handle]->getStateString().c_str() );
        DEBUG_OUT("\n");
    }
    DEBUG_OUT(simulations[handle]->getStateString().c_str() ); // show also what happened in last tick
    DEBUG_OUT("\n");

    return PyBool_FromLong(1L);
}

static PyObject *robosoc2d_simulatorIsValid(PyObject *self, PyObject *args, PyObject *keywds){
    static char *keywords[] = {(char *)"handle", NULL};
    int handle;
    if (!PyArg_ParseTupleAndKeywords(args, keywds, "i", keywords, &handle)){
        PyErr_SetString(PyExc_TypeError, "parameter must be an integer");
        return NULL;
    }

    if(simulations.count(handle) == 0){
        return PyBool_FromLong(0L);
    }

    return PyBool_FromLong(1L);
}

static PyObject *robosoc2d_simulatorGetStateString(PyObject *self, PyObject *args, PyObject *keywds){
    static char *keywords[] = {(char *)"handle", NULL};
    int handle;
    if (!PyArg_ParseTupleAndKeywords(args, keywds, "i", keywords, &handle)){
        PyErr_SetString(PyExc_TypeError, "parameter must be an integer");
        return NULL;
    }

    if(simulations.count(handle) == 0){
        PyErr_SetString(R2Error, "simulation handle not existing");
        return NULL;
    }

    return  PyUnicode_FromString(simulations[handle]->getStateString().c_str());
}

static PyObject *robosoc2d_simulatorGetGameState(PyObject *self, PyObject *args, PyObject *keywds){
    static char *keywords[] = {(char *)"handle", NULL};
    int handle;
    if (!PyArg_ParseTupleAndKeywords(args, keywds, "i", keywords, &handle)){
        PyErr_SetString(PyExc_TypeError, "parameter must be an integer");
        return NULL;
    }

    if(simulations.count(handle) == 0){
        PyErr_SetString(R2Error, "simulation handle not existing");
        return NULL;
    }

    return  pythonizeGameState(simulations[handle]->getGameState());
}

static PyObject *robosoc2d_simulatorGetRandomSeed(PyObject *self, PyObject *args, PyObject *keywds){
    static char *keywords[] = {(char *)"handle", NULL};
    int handle;
    if (!PyArg_ParseTupleAndKeywords(args, keywds, "i", keywords, &handle)){
        PyErr_SetString(PyExc_TypeError, "parameter must be an integer");
        return NULL;
    }

    if(simulations.count(handle) == 0){
        PyErr_SetString(R2Error, "simulation handle not existing");
        return NULL;
    }

    return  PyLong_FromLong(simulations[handle]->getRandomSeed());
}

static PyObject *robosoc2d_simulatorGetTeamNames(PyObject *self, PyObject *args, PyObject *keywds){
    static char *keywords[] = {(char *)"handle", NULL};
    int handle;
    if (!PyArg_ParseTupleAndKeywords(args, keywds, "i", keywords, &handle)){
        PyErr_SetString(PyExc_TypeError, "parameter must be an integer");
        return NULL;
    }

    if(simulations.count(handle) == 0){
        PyErr_SetString(R2Error, "simulation handle not existing");
        return NULL;
    }

    auto names=simulations[handle]->getTeamNames();
    PyObject *name1= PyUnicode_FromString(names[0].c_str());
    if(name1==NULL){
        return NULL;
    }
    PyObject *name2= PyUnicode_FromString(names[1].c_str());
    if(name2==NULL){
        Py_DECREF(name1);
        return NULL;
    }

    PyObject *pyNames = PyTuple_New(2);
    if(pyNames==NULL){
        Py_DECREF(name1);
        Py_DECREF(name2);
        return NULL;
    }
    PyTuple_SetItem(pyNames, 0, name1);
    PyTuple_SetItem(pyNames, 1, name2);
    
    return  pyNames;
}

static PyObject *robosoc2d_simulatorSaveStateHistory(PyObject *self, PyObject *args, PyObject *keywds){
    static char *keywords[] = {(char *)"handle", (char *)"filename", NULL};
    int handle;
    const char* filename;
    if (!PyArg_ParseTupleAndKeywords(args, keywds, "is", keywords, &handle, &filename)){
        PyErr_SetString(PyExc_TypeError, "wrong parameters");
        return NULL;
    }

    if(simulations.count(handle) == 0){
        PyErr_SetString(R2Error, "simulation handle not existing");
        return NULL;
    }

    return  PyBool_FromLong((long)simulations[handle]->saveStatesHistory(string(filename)));
}

static PyObject *robosoc2d_simulatorSaveActionsHistory(PyObject *self, PyObject *args, PyObject *keywds){
    static char *keywords[] = {(char *)"handle", (char *)"filename", NULL};
    int handle;
    const char* filename;
    if (!PyArg_ParseTupleAndKeywords(args, keywds, "is", keywords, &handle, &filename)){
        PyErr_SetString(PyExc_TypeError, "wrong arguments");
        return NULL;
    }

    if(simulations.count(handle) == 0){
        PyErr_SetString(R2Error, "simulation handle not existing");
        return NULL;
    }

    return  PyBool_FromLong((long)simulations[handle]->saveActionsHistory(string(filename)));
}

static PyObject *robosoc2d_simulatorDeleteAll(PyObject *self, PyObject *args){
    simulations.clear();
    Py_INCREF(Py_None);
    return Py_None;
}

// Sequence of robosoc2d.player: provided team1 with user's logic
// Sequence of robosoc2d.player: provided team2 with user's logic
// integer (optional): random seed to be used by the simulation random number generators
// reference to a robosoc2d.settings object (optional): settings to be used to build the simulation
static PyObject *robosoc2d_set_environment(PyObject *self, PyObject *args, PyObject *keywds) {
    static char *keywords[] = {(char *)"handle", (char *)"tick", (char *)"score1",  (char *)"score2", (char *)"state",  (char *)"ball_x",  (char *)"ball_y",  (char *)"ball_velocity_x",  (char *)"ball_velocity_y", (char *)"team1", (char *)"team2",  (char *)"last_touched_team2",  (char *)"ball_catched",  (char *)"ball_catched_team2", NULL};
    int handle=0;
    int tick=0;
    int score1=0;
    int score2=0;
    int state=0;
    double ballX=0;
    double ballY=0;
    double ballVelocityX=0;
    double ballVelocityY=0;
    PyObject *pTeamSequence[2]{NULL,NULL};
    int lastTouchedTeam2= 0;
    int ballCatched=0;
    int ballCatchedTeam2=0;

    PyObject *pObj=NULL;

    R2SettingsObject *pSettings;
    R2EnvSettings cSettings;

    if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiiiiddddOO|pip", keywords, &handle, &tick, &score1, &score2, &state, &ballX, &ballY, &ballVelocityX, &ballVelocityY, &pTeamSequence[0], &pTeamSequence[1], &lastTouchedTeam2, &ballCatched, &ballCatchedTeam2)){
        PyErr_SetString(PyExc_TypeError, "wrong parameters");
        return NULL;
    }

    if(simulations.count(handle) == 0){
        PyErr_SetString(R2Error, "simulation handle not existing");
        return NULL;
    }

    R2State rstate= static_cast<R2State>(state);
    R2ObjectInfo ball(ballX, ballY, ballVelocityX, ballVelocityY);

    std::vector<R2PlayerInfo> team[2];

    for(int t=0; t<=1; t++){
        if(!PySequence_Check(pTeamSequence[t])){
            PyErr_SetString(PyExc_TypeError, "first and second arguments must be a sequence of players");
            return NULL;
        }

        int len=(int)PySequence_Length(pTeamSequence[t]);

        for (int i = 0; i < len; i++) { 
            PyObject *player = PySequence_GetItem(pTeamSequence[t], i);
            if (player == NULL){ // some failure
                PyErr_SetString(PyExc_TypeError, "unable to get player object from sequence");
                return NULL;
            }
            
            R2PlayerInfoObject *playerInfoObject= (R2PlayerInfoObject *)player;
            R2PlayerInfo playerInfo;
            setR2PlayerInfo(playerInfo, *playerInfoObject);
            team[t].push_back(playerInfo);
            Py_DECREF(player);
        }
    }

    simulations[handle]->setEnvironment(tick, score1, score2, rstate, ball, 
        team[0], team[1], (bool) lastTouchedTeam2, ballCatched, (bool) ballCatchedTeam2) ;

   Py_INCREF(Py_None);
   return Py_None;
}

// helper method for people using python prior to 3.7 because python<3.7 doesn't have true IEEE754 remainder (numpy neither)
// this should be equivalent to python 3.7: math.remainder(dividend, divisor)
static PyObject *robosoc2d_remainder(PyObject *self, PyObject *args, PyObject *keywds){
    static char *keywords[] = {(char *)"dividend", (char *)"divisor", NULL};
    double dividend,divisor;
    if (!PyArg_ParseTupleAndKeywords(args, keywds, "dd", keywords, &dividend, &divisor)){
        PyErr_SetString(PyExc_TypeError, "wrong parameters, they should be two floats");
        return NULL;
    }

    return  PyFloat_FromDouble(remainder(dividend, divisor));
}


static PyMethodDef Robosoc2dMethods[] = { 
    {"get_version", (PyCFunction)robosoc2d_getVersion, METH_NOARGS, "get_version()\n\nIt returns a string containing the version of the simulator.\nIt uses the format: \"major.minor.revision\". No parameters." },
    {"get_default_settings", (PyCFunction)robosoc2d_getDefaultSettings, METH_NOARGS, "get_default_settings()\n\nIt returns a settings object containing the default settings of simulator. No parameters." },
    {"get_seed_by_current_time", (PyCFunction)robosoc2d_getSeedByCurrentTime, METH_NOARGS, "get_seed_by_current_time()\n\nIt returns a random seed generated by current time. No parameters." },
    {"build_simulator", (PyCFunction)robosoc2d_buildSimulator, METH_VARARGS|METH_KEYWORDS, "build_simulator(team1, team2, team1name, team2name, random_seed, game_settings)\n\nIt creates a simulator and returns an integer that represents an handle to it. First and second parameters are mandatory and must be sequences of players, one sequence per team. It is possible to have a different number of players in each team. The other parameters are not mandatory. The third and fourth parameters are strings containing team names. Fifth parameter is an integer containing the random seed to be used to initialize the random engine (if this parameter is missing, a random seed will be generated depending on current time). Sixth parameter is a settings object in case you want to choose you own settings. (a player is an object that implements the method step(self, env, pitch, settings, team1, team2) that receives the information about the game and returns the choosen action as a tuple composed by one integer and three floats) " },
    {"build_simpleplayer_simulator", (PyCFunction)robosoc2d_buildSimplePlayerSimulator, METH_VARARGS|METH_KEYWORDS, "build_simpleplayer_simulator (team1, how_manysimpleplayers_team1, team2, how_manysimpleplayers_team1, team1name, team2name, random_seed, game_settings)\n\nIt creates a simulator and returns an integer that represents an handle to it. For each team it is possible to use the built-in class SimplePlayer for some players. The user may decide how many SimplePlayer agent each team may have: the first players of the team will be the SimplePlayers ones (if any), and the subsequent players will be the ones inserted in the team sequences (that may possibly be empty). Since the SimplePlayers will be the first players of the team, and since the first player plays in the goalkeeper role, if a team has at least a SimplePlayer, it means the the goalkeeper will be certainly a SimplePlater. Parameters 1-4 are mandatory. First parameter must be a sequence containing the players objects for the first team, and second parameter is a boolean determining how many SimplePlayers have to be added at the beginning of the team. Third and fourth parameters are the same for the second team. It is possible to have a different number of players in each team. The other parameters are not mandatory. The fifth and sixth parameters are strings containing team names. Seventh parameter is an integer containing the random seed to be used to initialize the random engine (if this parameter is missing, a random seed will be generated depending on current time). Eighth parameter is a settings object in case you want to choose you own settings.(a player is an object that implements the method step(self, env, pitch, settings, team1, team2) that receives the information about the game and returns the choosen action as a tuple composed by one integer and three floats) " },
    {"simulator_step_if_playing", (PyCFunction)robosoc2d_simulatorStepIfPlaying, METH_VARARGS|METH_KEYWORDS, "simulator_step_if_playing (handle)\n\nIt runs a step of the simulation, if the simulation is still playable and not terminated. It accepts only one parameter: an integer that is an handle to the simulation. It returns a boolean that is false if the game was still playable." },
    {"simulator_play_game", (PyCFunction)robosoc2d_simulatorPlayGame, METH_VARARGS|METH_KEYWORDS, "simulator_play_game (handle)\n\nIt runs all the step of the simulation till the end. It accepts only one parameter: an integer that is an handle to the simulation. It returns a boolean containing True if the game was played, throws an exception otherwise." },
    {"simulator_delete", (PyCFunction)robosoc2d_simulatorDelete, METH_VARARGS|METH_KEYWORDS, "simulator_delete (handle)\n\nIt deletes a simulator. It accepts only one parameter: an integer that is an handle to the simulation. It returns a boolean containing True if the simulation has been deleted, False otherwise." },
    {"simulator_delete_all", (PyCFunction)robosoc2d_simulatorDeleteAll, METH_NOARGS, "simulator_delete_all ()\n\nIt deletes all simulators. No parameters. It returns None" },
    {"simulator_is_valid", (PyCFunction)robosoc2d_simulatorIsValid, METH_VARARGS|METH_KEYWORDS,"simulator_is_valid (handle)\n\nIt checks if handle passed as parameter refers to an existing simulator, and returns a boolean accordly"},
    {"simulator_get_state_string", (PyCFunction)robosoc2d_simulatorGetStateString, METH_VARARGS|METH_KEYWORDS,"simulator_get_state_string (handle)\n\nIt returns the state string of simulator. It accepts only one parameter: an integer that is an handle to the simulation."},
    {"simulator_get_game_state", (PyCFunction)robosoc2d_simulatorGetGameState, METH_VARARGS|METH_KEYWORDS,"simulator_get_game_state (handle)\n\nIt returns the game state of the simulator. It accepts only one parameter: an integer that is an handle to the simulation. It returns a tuple containing 5 objects: the first is an environment object, the second is a pitch object, the third is a settings object, the fourth is a tuple of player_info objects containing the infromation about first team players, and the fifth object is a tuple of player_info for the secondo team."},
    {"simulator_get_random_seed", (PyCFunction)robosoc2d_simulatorGetRandomSeed, METH_VARARGS|METH_KEYWORDS,"simulator_get_random_seed (handle)\n\nIt returns the random seed of the simulator. It accepts only one parameter: an integer that is an handle to the simulation."},
    {"simulator_get_team_names", (PyCFunction)robosoc2d_simulatorGetTeamNames, METH_VARARGS|METH_KEYWORDS,"simulator_get_team_names (handle)\n\nIt returns the team names as a tuple containing two strings. It accepts only one parameter: an integer that is an handle to the simulation."},
    {"simulator_save_state_history", (PyCFunction)robosoc2d_simulatorSaveStateHistory, METH_VARARGS|METH_KEYWORDS,"simulator_save_state_history (handle, filename)\n\nIt saves the state history of the simulator. The first parameter is an integer that is an handle to the simulation. The second parameter is the file name. It returns a boolean representing success (True) or failure (False)."},
    {"simulator_save_actions_history", (PyCFunction)robosoc2d_simulatorSaveActionsHistory, METH_VARARGS|METH_KEYWORDS,"simulator_save_actions_history (handle, filename)\n\nIt saves the actions history of the simulator. The first parameter is an integer that is an handle to the simulation. The second parameter is the file name. It returns a boolean representing success (True) or failure (False)."},
    {"simulator_set_environment", (PyCFunction)robosoc2d_set_environment, METH_VARARGS|METH_KEYWORDS, "simulator_set_environment(handle, tick, score1, score2, state, ball_x, ball_y, ball_velocity_x, ball_velocity_y, team1, team2, last_touched_team2, ball_catched, ball_catched_team2) \n\n It sets the game configuration, making possible to decide things like players' and ball's positions, velocities and so on. The first parameter is an integer that is an handle to the simulation. The second parameter is an integer that represents the time tick. Third and fourth parameters are integers representing current score for the two teams. The fith parameter is an integer indicating the state of the game, to be picked up among the constants robosoc2d.STATE_* , for instance robosoc2d.STATE_PLAY . Next 4 parameters are floats containing ball position and velocity. Then the parameter team1 is a sequence containing objects of type robosoc2d.player_info that specify position, velocity and direction of the players of the first team. The parameter team2 is the same for the second team. Then there are 3 optional parameters: last_touched_team2 is a bool that indicates if the last team that touched the ball was team2. The integer ball_catched indicates if the ball is currently owned by a gall keeper: if >0 the ball is catched and only the goalkeeper can kick it and the ball moves with him, thenumber actually indicates for how many ticks the ball is still allowed to be possessed by the goalkeeper. The boolean ball_catched_team2 should be true if the goalkeeper that owns the ball is the one of second team."},
    {"remainder", (PyCFunction)robosoc2d_remainder, METH_VARARGS|METH_KEYWORDS,"remainder (dividend, divisor)\n\nMath remainder function following IEEE754 specification. Helper method for people using python prior to 3.7 because python<3.7 doesn't have true IEEE754 remainder (numpy neither). This should be equivalent to python 3.7: math.remainder(dividend, divisor)"},
    
    {NULL, NULL, 0, NULL}        /* Sentinel */
};

static void robosoc2d_exitModule(){
    isPythonActive=false;    
    //Python environment is already off at this point
    //each python object referenced/pointed by C++ object is not valid anymore (no matter what the reference count is)
    //and no Python API should be called at this point
    // see https://docs.python.org/3/c-api/sys.html#c.Py_AtExit
}


const char *robosoc2dmodule_doc="robosoc2d\n\n\
Very Simplified 2D Robotic Soccer Simulator (c) 2021 Ruggero Rossi\n\n\
Classes: \n\
settings \n\
pitch \n\
environment \n\
player_info \n\
error \n\
\n\
Functions: \n\
get_default_settings () \n\
get_seed_by_current_time () \n\
build_simulator (team1, team2, team1name, team2name, random_seed, game_settings) \n\
build_simpleplayer_simulator (team1, how_manysimpleplayers_team1, team2, how_manysimpleplayers_team1, team1name, team2name, random_seed, game_settings) \n\
simulator_step_if_playing (handle) \n\
simulator_play_game (handle) \n\
simulator_delete (handle) \n\
simulator_delete_all () \n\
simulator_is_valid (handle) \n\
simulator_get_state_string (handle) \n\
simulator_get_game_state (handle) \n\
simulator_get_random_seed (handle) \n\
simulator_get_team_names (handle) \n\
simulator_save_state_history (handle, filename) \n\
simulator_save_actions_history (handle, filename) \n\
remainder (dividend, divisor) \n\
\n\
Constants: \n\
STATE_INACTIVE \n\
STATE_READY \n\
STATE_KICKOFF1 \n\
STATE_KICKOFF2 \n\
STATE_PLAY \n\
STATE_STOPPED \n\
STATE_GOALKICK1UP \n\
STATE_GOALKICK1DOWN \n\
STATE_GOALKICK2UP \n\
STATE_GOALKICK2DOWN \n\
STATE_CORNER1UP \n\
STATE_CORNER1DOWN \n\
STATE_CORNER2UP \n\
STATE_CORNER2DOWN \n\
STATE_THROWIN1 \n\
STATE_THROWIN2 \n\
STATE_PAUSED \n\
STATE_HALFTIME \n\
STATE_GOAL1 \n\
STATE_GOAL2 \n\
STATE_ENDED \n\
ACTION_NOOP \n\
ACTION_MOVE \n\
ACTION_DASH \n\
ACTION_TURN \n\
ACTION_KICK \n\
ACTION_CATCH \n";

static struct PyModuleDef robosoc2dmodule = {
    PyModuleDef_HEAD_INIT,
    "robosoc2d",   /* name of module */
    robosoc2dmodule_doc,   //doc
    -1, // per interpreter size
    Robosoc2dMethods
};

PyMODINIT_FUNC
PyInit_robosoc2d(void) {
    R2SettingsType.tp_name = "robosoc2d.settings";
    R2SettingsType.tp_basicsize = sizeof(R2SettingsObject);
    R2SettingsType.tp_itemsize = 0;
    R2SettingsType.tp_flags = Py_TPFLAGS_DEFAULT;
    R2SettingsType.tp_doc = R2Settings_doc;
    R2SettingsType.tp_members = R2Settings_members;
    R2SettingsType.tp_methods = R2Settings_methods;
    R2SettingsType.tp_new = PyType_GenericNew;
    R2SettingsType.tp_repr = (reprfunc) R2SettingsObject_repr;
    R2SettingsType.tp_dealloc = (destructor) R2SettingsType_dealloc;    //unnecessary

    R2PitchType.tp_name = "robosoc2d.pitch";
    R2PitchType.tp_basicsize = sizeof(R2PitchObject);
    R2PitchType.tp_itemsize = 0;
    R2PitchType.tp_flags = Py_TPFLAGS_DEFAULT;
    R2PitchType.tp_doc = R2Pitch_doc;
    R2PitchType.tp_members = R2Pitch_members;
    R2PitchType.tp_methods = R2Pitch_methods;
    R2PitchType.tp_new = PyType_GenericNew;
    R2PitchType.tp_repr = (reprfunc) R2PitchObject_repr;
    R2PitchType.tp_dealloc = (destructor) R2PitchType_dealloc;  //unnecessary

    R2EnvironmentType.tp_name = "robosoc2d.environment";
    R2EnvironmentType.tp_basicsize = sizeof(R2EnvironmentObject);
    R2EnvironmentType.tp_itemsize = 0;
    R2EnvironmentType.tp_flags = Py_TPFLAGS_DEFAULT;
    R2EnvironmentType.tp_doc = R2Environment_doc;
    R2EnvironmentType.tp_members = R2Environment_members;
    R2EnvironmentType.tp_methods = R2Environment_methods;
    R2EnvironmentType.tp_new = PyType_GenericNew;
    R2EnvironmentType.tp_repr = (reprfunc) R2EnvironmentObject_repr;
    R2EnvironmentType.tp_dealloc = (destructor) R2EnvironmentType_dealloc;  //unnecessary

    R2PlayerInfoType.tp_name = "robosoc2d.player_info";
    R2PlayerInfoType.tp_basicsize = sizeof(R2PlayerInfoObject);
    R2PlayerInfoType.tp_itemsize = 0;
    R2PlayerInfoType.tp_flags = Py_TPFLAGS_DEFAULT;
    R2PlayerInfoType.tp_doc = R2PlayerInfo_doc;
    R2PlayerInfoType.tp_members = R2PlayerInfo_members;
    R2PlayerInfoType.tp_methods = R2PlayerInfo_methods;
    R2PlayerInfoType.tp_new = PyType_GenericNew;
    R2PlayerInfoType.tp_repr = (reprfunc) R2PlayerInfoObject_repr;
    R2PlayerInfoType.tp_dealloc = (destructor) R2PlayerInfoType_dealloc;    //unnecessary


    if (PyType_Ready(&R2SettingsType) < 0)
    {
        return NULL;
    }

    if (PyType_Ready(&R2PitchType) < 0)
    {
        return NULL;
    }

    if (PyType_Ready(&R2EnvironmentType) < 0)
    {
        return NULL;
    }

    if (PyType_Ready(&R2PlayerInfoType) < 0)
    {
        return NULL;
    }

    PyObject* m = PyModule_Create(&robosoc2dmodule);
    if (m == NULL) {
        return NULL;
    }

    R2Error = PyErr_NewException("robosoc2d.error", NULL, NULL);
    Py_XINCREF(R2Error);
    if (PyModule_AddObject(m, "error", R2Error) < 0) {
        Py_XDECREF(R2Error);
        Py_CLEAR(R2Error);
        Py_DECREF(m);
        return NULL;
    }
    
    Py_INCREF(&R2SettingsType);
    if (PyModule_AddObject(m, "settings", (PyObject *) &R2SettingsType) < 0) {
        Py_DECREF(&R2SettingsType);
        Py_DECREF(R2Error);
        Py_CLEAR(R2Error);
        Py_DECREF(m);
        return NULL;
    }

    Py_INCREF(&R2PitchType);
    if (PyModule_AddObject(m, "pitch", (PyObject *) &R2PitchType) < 0) {
        Py_DECREF(&R2SettingsType);
        Py_DECREF(&R2PitchType);
        Py_DECREF(R2Error);
        Py_CLEAR(R2Error);
        Py_DECREF(m);
        return NULL;
    }

    Py_INCREF(&R2EnvironmentType);
    if (PyModule_AddObject(m, "environment", (PyObject *) &R2EnvironmentType) < 0) {
        Py_DECREF(&R2SettingsType);
        Py_DECREF(&R2PitchType);
        Py_DECREF(&R2EnvironmentType);
        Py_DECREF(R2Error);
        Py_CLEAR(R2Error);
        Py_DECREF(m);
        return NULL;
    }

    Py_INCREF(&R2PlayerInfoType);
    if (PyModule_AddObject(m, "player_info", (PyObject *) &R2PlayerInfoType) < 0) {
        Py_DECREF(&R2SettingsType);
        Py_DECREF(&R2PitchType);
        Py_DECREF(&R2EnvironmentType);
        Py_DECREF(&R2PlayerInfoType);
        Py_DECREF(R2Error);
        Py_CLEAR(R2Error);
        Py_DECREF(m);
        return NULL;
    }

    stepMethodName =  PyUnicode_FromString("step"); 
    if(stepMethodName==NULL)
    {
        Py_DECREF(&R2SettingsType);
        Py_DECREF(&R2PitchType);
        Py_DECREF(&R2EnvironmentType);
        Py_DECREF(&R2PlayerInfoType);
        Py_DECREF(R2Error);
        Py_CLEAR(R2Error);
        Py_DECREF(m);
        return NULL;
    }
    Py_INCREF(stepMethodName);

    if (
	    PyModule_AddIntConstant(m, "STATE_INACTIVE", static_cast<int>(R2State::Inactive)) ||
	    PyModule_AddIntConstant(m, "STATE_READY", static_cast<int>(R2State::Ready)) ||
	    PyModule_AddIntConstant(m, "STATE_KICKOFF1", static_cast<int>(R2State::Kickoff1)) ||
	    PyModule_AddIntConstant(m, "STATE_KICKOFF2", static_cast<int>(R2State::Kickoff2)) ||
	    PyModule_AddIntConstant(m, "STATE_PLAY", static_cast<int>(R2State::Play)) ||
	    PyModule_AddIntConstant(m, "STATE_STOPPED", static_cast<int>(R2State::Stopped)) ||
	    PyModule_AddIntConstant(m, "STATE_GOALKICK1UP", static_cast<int>(R2State::Goalkick1up)) ||
	    PyModule_AddIntConstant(m, "STATE_GOALKICK1DOWN", static_cast<int>(R2State::Goalkick1down)) ||
	    PyModule_AddIntConstant(m, "STATE_GOALKICK2UP", static_cast<int>(R2State::Goalkick2up)) ||
	    PyModule_AddIntConstant(m, "STATE_GOALKICK2DOWN", static_cast<int>(R2State::Goalkick2down)) ||
	    PyModule_AddIntConstant(m, "STATE_CORNER1UP", static_cast<int>(R2State::Corner1up)) ||
	    PyModule_AddIntConstant(m, "STATE_CORNER1DOWN", static_cast<int>(R2State::Corner1down)) ||
	    PyModule_AddIntConstant(m, "STATE_CORNER2UP", static_cast<int>(R2State::Corner2up)) ||
	    PyModule_AddIntConstant(m, "STATE_CORNER2DOWN", static_cast<int>(R2State::Corner2down)) ||
	    PyModule_AddIntConstant(m, "STATE_THROWIN1", static_cast<int>(R2State::Throwin1)) ||
	    PyModule_AddIntConstant(m, "STATE_THROWIN2", static_cast<int>(R2State::Throwin2)) ||
	    PyModule_AddIntConstant(m, "STATE_PAUSED", static_cast<int>(R2State::Paused)) ||
	    PyModule_AddIntConstant(m, "STATE_HALFTIME", static_cast<int>(R2State::Halftime)) ||
	    PyModule_AddIntConstant(m, "STATE_GOAL1", static_cast<int>(R2State::Goal1)) ||
	    PyModule_AddIntConstant(m, "STATE_GOAL2", static_cast<int>(R2State::Goal2)) ||
	    PyModule_AddIntConstant(m, "STATE_ENDED", static_cast<int>(R2State::Ended)) ||

	    PyModule_AddIntConstant(m, "ACTION_NOOP", static_cast<int>(R2ActionType::NoOp)) ||
	    PyModule_AddIntConstant(m, "ACTION_MOVE", static_cast<int>(R2ActionType::Move)) ||
	    PyModule_AddIntConstant(m, "ACTION_DASH", static_cast<int>(R2ActionType::Dash)) ||
	    PyModule_AddIntConstant(m, "ACTION_TURN", static_cast<int>(R2ActionType::Turn)) ||
	    PyModule_AddIntConstant(m, "ACTION_KICK", static_cast<int>(R2ActionType::Kick)) ||
	    PyModule_AddIntConstant(m, "ACTION_CATCH", static_cast<int>(R2ActionType::Catch)) 
	) {
        Py_DECREF(stepMethodName);
        Py_DECREF(&R2SettingsType);
        Py_DECREF(&R2PitchType);
        Py_DECREF(&R2EnvironmentType);
        Py_DECREF(&R2PlayerInfoType);
        Py_DECREF(R2Error);
        Py_CLEAR(R2Error);
        Py_DECREF(m);
        return NULL;
    }

    Py_AtExit(robosoc2d_exitModule);

    isPythonActive=true;

    return m;
}

