#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include <algorithm>
#include <array>
#include <sys/time.h>

using namespace std;
#define my_delete(x) \
    {                \
        delete x;    \
        x = NULL;    \
    }

// *** DECLARATION ***
class Vector2D;
class Checkpoint;
class Command;
class Drone;
void cheapPlay(Command* cmd, Drone* drone);

// *** CONSTANTS ***
const int SIM_DEPTH = 5;
const int GEN_POP_SIZE = 256;
const int SHIELD_TIMER = 3;
const int CP_RADIUS = 550;
const int DRONE_RADIUS = 400;
const double FRICTION = 0.85f;
const float PI = 3.1415926535897932384626434;

// *** GLOBAL ***
struct timeval timestart, timeend;
int laps, checkpointCount, turn, totalCp;
vector<Checkpoint*> checkPoints;
vector<Drone*> drones;
vector<Drone*> dronePositions;
int MYRUNNER, MYSUPPORT, HISRUNNER, HISSUPPORT;
int MYTIMEOUT = 100, HISTIMEOUT = 100;
int STRATEGY;
double COS[2 * 360];
double SIN[2 * 360];

// *** DEBUG ***
int BOUNCE_CALL = 0;
int MOVDUR_CALL = 0;
int SIM_BRAN = 0;
int cpt = 0;

// ***********************************************************

float elapsedMS()
{
    gettimeofday(&timeend, NULL);
    return ((timeend.tv_sec - timestart.tv_sec) * 1000 + (timeend.tv_usec - timestart.tv_usec) / 1000.0) + 0.5;
}

float degToRad(float x)
{
    return x / 180 * PI;
}

float radToDeg(float x)
{
    return x / PI * 180;
}

int randInt(int a, int b)
{
    return rand() % (b - a) + a;
}

double randDouble(double a, double b)
{
    return (rand() / (double)RAND_MAX) * (b - a) + a;
}

double precos(int angle)
{
    while (angle < 0) {
        angle += 360;
    }
    while (angle > 360) {
        angle -= 360;
    }
    return COS[angle];
}

double presin(int angle)
{
    while (angle < 0) {
        angle += 360;
    }
    while (angle > 360) {
        angle -= 360;
    }
    return SIN[angle];
}

void precalculateCosSin()
{
    int idx = 0;
    for (int i = -359; i <= 360; i++) {
        double rad = degToRad((float)i);
        COS[idx] = cos(rad);
        SIN[idx] = sin(rad);
        idx++;
    }
}

// ***********************************************************

class Checkpoint {
public:
    int x, y;

    Checkpoint(int x, int y)
    {
        this->x = x;
        this->y = y;
    }
};

// ***********************************************************

class Vector2D {
public:
    double dX, dY;

    Vector2D()
    {
        this->dX = 0.0;
        this->dY = 0.0;
    }

    Vector2D(Vector2D* from)
    {
        this->dX = from->dX;
        this->dY = from->dY;
    }

    Vector2D(double dX, double dY)
    {
        this->dX = dX;
        this->dY = dY;
    }

    void copy(Vector2D* v)
    {
        this->dX = v->dX;
        this->dY = v->dY;
    }

    double sqrLength()
    {
        return this->dX * this->dX + this->dY * this->dY;
    }

    double length()
    {
        return sqrt(this->dX * this->dX + this->dY * this->dY);
    }

    void add(Vector2D* v1)
    {
        this->dX += v1->dX;
        this->dY += v1->dY;
    }

    void add(Vector2D* v1, double factor)
    {
        this->dX += v1->dX * factor;
        this->dY += v1->dY * factor;
    }

    void sub(Vector2D* v1)
    {
        this->dX -= v1->dX;
        this->dY -= v1->dY;
    }

    void sub(Vector2D* v1, double factor)
    {
        this->dX -= v1->dX * factor;
        this->dY -= v1->dY * factor;
    }

    void scale(double scaleFactor)
    {
        this->dX *= scaleFactor;
        this->dY *= scaleFactor;
    }

    void normalize()
    {
        double length = sqrt(this->dX * this->dX + this->dY * this->dY);
        if (length != 0) {
            this->dX /= length;
            this->dY /= length;
        }
    }

    double dotProduct(Vector2D& v1)
    {
        return this->dX * v1.dX + this->dY * v1.dY;
    }
};

// ***********************************************************

class Command {
public:
    int angle, th;
    bool shield;

    Command(int angle, int th, bool shield)
    {
        this->angle = angle;
        this->th = th;
        this->shield = shield;
    }

    void copy(Command* from)
    {
        this->th = from->th;
        this->angle = from->angle;
        this->shield = from->shield;
    }

    void randomize(bool allowShield)
    {
        angle = randInt(0, 37) - 18;
        if (angle > 18)
            angle = 18;
        if (angle < -18)
            angle = -18;
        if (allowShield && randInt(0, 101) > 80) {
            shield = true;
            th = -1;
        }
        else {
            shield = false;
            th = randInt(0, 101);
        }
    }

    void smallMutation(bool allowShield)
    {
        if (randInt(0, 101) > 50) {
            angle += randInt(0, 7) - 3;
            if (angle > 18)
                angle = 18;
            if (angle < -18)
                angle = -18;
        }
        else {
            if (allowShield && randInt(0, 101) > 90) {
                shield = true;
                th = -1;
            }
            else {
                shield = false;
                th += randInt(0, 15) - 5;
                if (th > 100)
                    th = 100;
                if (th < 0)
                    th = 0;
            }
        }
    }
};

// ***********************************************************

class Drone {
public:
    int id, cpTodo, rotation, shieldTimer, nextCp, nextNextCp;
    bool mine;
    double x, y;
    Vector2D* speed;

    ~Drone()
    {
        my_delete(this->speed);
    }

    Drone(int id)
    {
        this->nextCp = -1;
        this->nextNextCp = -1;
        this->id = id;
        this->mine = id < 2;
        this->speed = new Vector2D();
    }

    void copy(Drone* from)
    {
        this->shieldTimer = from->shieldTimer;
        this->id = from->id;
        this->mine = from->mine;
        this->rotation = from->rotation;
        this->x = from->x;
        this->y = from->y;
        this->speed->copy(from->speed);
        this->cpTodo = from->cpTodo;
        this->nextCp = from->nextCp;
        this->nextNextCp = from->nextNextCp;
    }

    void setCheapRaceCmd(Command* cmd)
    {
        if (nextCp == -1) {
            cmd->th = 0;
            cmd->angle = 0;
            cmd->shield = false;
            return;
        }
        Checkpoint* cp = checkPoints[nextCp];
        int degAngle = angleWithPoint(cp->x, cp->y);
        int th = 100;
        int sqrCpD = getNextCpSquaredD();

        if (abs(degAngle) > 90) {
            th=20;
        } else if(sqrCpD < 1000000) {
            th=55;
        } else if (sqrCpD < 250000) {
            th=40;
        } else {
            th=100;
        }

        if (degAngle < -18) {
            degAngle = -18;
        }
        if (degAngle > 18) {
            degAngle = 18;
        }

        cmd->th = th;
        cmd->angle = degAngle;
        cmd->shield = false;
    }

    void setCheapSupportCmd(Command* cmd, Drone* target)
    {
        int degAngle = angleWithPoint(target->x, target->y);
        int th = 100;

        if (abs(degAngle) > 90) {
            th=20;
        }

        if (degAngle < -18) {
            degAngle = -18;
        }
        if (degAngle > 18) {
            degAngle = 18;
        }

        cmd->th = th;
        cmd->angle = degAngle;
        cmd->shield = false;
    }

    void setCPCmd(Command* cmd, Checkpoint* cp, int th)
    {
        if (cp == NULL) {
            cmd->th = 0;
            cmd->angle = 0;
            cmd->shield = false;
            return;
        }
        int degAngle = angleWithPoint(cp->x, cp->y);
        if (turn > 0) {
            if (degAngle < -18) {
                degAngle = -18;
            }
            if (degAngle > 18) {
                degAngle = 18;
            }
        }
        cmd->th = th;
        cmd->angle = degAngle;
        cmd->shield = false;
    }

    void setNextCPCmd(Command* cmd, int th)
    {
        if (nextCp == -1) {
            cmd->th = 0;
            cmd->angle = 0;
            cmd->shield = false;
            return;
        }
        setCPCmd(cmd, checkPoints[nextCp], th);
    }

    void updateCpState()
    {
        int squaredDistance = getNextCpSquaredD();
        if (nextCp != -1 && squaredDistance <= CP_RADIUS * CP_RADIUS) {
            cpTodo--;
            nextCp = nextNextCp;
            if (nextCp != -1) {
                nextNextCp = (nextCp + 1) % checkpointCount;
            }
            else {
                nextNextCp = -1;
            }
        }
    }

    void moveDuring(double t)
    {
        MOVDUR_CALL++;
        x += speed->dX * t;
        y += speed->dY * t;
    }

    double collisionTime(Drone* drone)
    {
        double a, b, c, d;

        a = (speed->dX - drone->speed->dX) * (speed->dX - drone->speed->dX) + (speed->dY - drone->speed->dY) * (speed->dY - drone->speed->dY);
        b = 2 * (x * speed->dX - drone->x * speed->dX - x * drone->speed->dX + drone->x * drone->speed->dX + y * speed->dY - drone->y * speed->dY - y * drone->speed->dY + drone->y * drone->speed->dY);
        c = (drone->x - x) * (drone->x - x) + (drone->y - y) * (drone->y - y) - (DRONE_RADIUS + DRONE_RADIUS) * (DRONE_RADIUS + DRONE_RADIUS);
        d = b * b - 4 * a * c;

        if (a == 0 || d < 0) {
            return 999999;
        }
        else {
            double t = (-b - sqrt(d)) / (2 * a);
            return t > 0 ? t : 999999;
        }
    }

    int angleWithPoint(int tx, int ty)
    {
        double radAngle = atan2(ty - y, tx - x) - atan2(presin(rotation), precos(rotation));
        int result = radToDeg(radAngle);
        result += (result > 180) ? -360 : (result < -180) ? 360 : 0;
        return result;
    }

    int getPointSquaredD(int tx, int ty)
    {
        return (x - tx) * (x - tx) + (y - ty) * (y - ty);
    }

    int getNextCpSquaredD()
    {
        if (nextCp == -1) {
            return 0;
        }
        else {
            return getPointSquaredD(checkPoints[nextCp]->x, checkPoints[nextCp]->y);
        }
    }
};
// ***********************************************************

class Chromosome {
public:
    Command* commands[SIM_DEPTH * 4]; // Les commandes des drones [turn*4 + droneId]
    double score;

    ~Chromosome()
    {
        for (int i = 0; i < SIM_DEPTH * 4; i++) {
            delete commands[i];
        }
    }

    Chromosome()
    {
        for (int i = 0; i < SIM_DEPTH * 4; i++) {
            commands[i] = new Command(0, 0, false);
        }
    }

    void copy(Chromosome* ch)
    {
        for (int turn = 0; turn < SIM_DEPTH; turn++) {
            for (int did = 0; did < 2; did++) {
                this->commands[turn * 4 + did]->copy(ch->commands[turn * 4 + did]);
            }
            // for (int did = 2; did < 4; did++) {
            //     this->commands[turn * 4 + did] = NULL;
            // }
        }
    }

    bool shieldUser()
    {
        return commands[0]->shield || commands[1]->shield;
    }
};

// ***********************************************************

class Simulator {
public:
    Drone* testedDrones[4]; // Les drones à utiliser pour la simulation
    Chromosome* chromosome;

    ~Simulator()
    {
        for (int i = 0; i < 4; i++) {
            delete testedDrones[i];
        }
    }

    Simulator(Chromosome* chromosome)
    {
        for (int i = 0; i < 4; i++) {
            testedDrones[i] = new Drone(i);
            testedDrones[i]->copy(drones[i]);
        }
        this->chromosome = chromosome;
    }

    /**
     * 1    YOLO        Tout faire pour empecher le runner ennemi de traverser son prochain CP
     * .                Maximiser distance entre lui et son CP. Puis minimiser distance entre mon runner et lui, et entre mon chaser et son CP.
     * <p>
     * 2    CLASSIC1    Priorité au score de mon runner. Maximiser distance entre runner adverse et son CP. Puis minimiser distance entre mon chaser et le runner adverse.
     * <p>
     * 3    CLASSIC1    Priorité au score de mon runner. Maximiser distance entre runner adverse et son CP. Puis minimiser distance entre mon chaser et le prochain CP du runner adverse.
     * .
     * 4    FINISH1     I'm ahead and I have 1 CP left. Rammener mon chaser en support.
     */
    double evaluate()
    {
        double score = 0;

        Drone* tMyRunner = testedDrones[MYRUNNER];
        Drone* tMySupport = testedDrones[MYSUPPORT];
        Drone* tHisRunner = testedDrones[HISRUNNER];
        Drone* tHisSupport = testedDrones[HISSUPPORT];

        /**
         * Je suis plus pret de son NCP que lui -> TARGET HIM
         * Il est plus pret de son NCP que moi -> TARGET NNCP
         */

        switch (STRATEGY) {
        case 1: {
            score += 1 * getDistanceScore(testedDrones[MYSUPPORT], (int)testedDrones[HISRUNNER]->x, (int)testedDrones[HISRUNNER]->y);
            score += 1 * getDistanceScore(testedDrones[MYRUNNER], (int)testedDrones[HISRUNNER]->x, (int)testedDrones[HISRUNNER]->y);
            score -= 8 * getRaceScore(testedDrones[HISRUNNER]);
            break;
        }
        case 2: {
            score += 6 * getRaceScore(testedDrones[MYRUNNER]);
            score -= 3 * getRaceScore(testedDrones[HISRUNNER]);
            score += 0.5 * getDistanceScore(testedDrones[MYSUPPORT], (int)testedDrones[HISRUNNER]->x, (int)testedDrones[HISRUNNER]->y);
            break;
        }
        case 3: {
            score += 6 * getRaceScore(testedDrones[MYRUNNER]);
            score -= 3 * getRaceScore(testedDrones[HISRUNNER]);
            if (testedDrones[HISRUNNER]->nextCp != -1) {
                score += 0.5 * getDistanceScore(testedDrones[MYSUPPORT], checkPoints[testedDrones[HISRUNNER]->nextCp]->x, checkPoints[testedDrones[HISRUNNER]->nextCp]->y);
            }
            else {
                score += 0.5 * getDistanceScore(testedDrones[MYSUPPORT], (int)testedDrones[HISRUNNER]->x, (int)testedDrones[HISRUNNER]->y);
            }
            int angle = abs(testedDrones[MYSUPPORT]->angleWithPoint((int)testedDrones[HISRUNNER]->x, (int)testedDrones[HISRUNNER]->y));
            score -= (((double)10) * angle) / 180;
            break;
        }
        case 4: {
            score += getRaceScore(tMyRunner);
            score += getRaceScore(tMySupport);
            break;
        }
        }

        return score;
    }

    double getRaceScore(Drone* drone)
    {
        double maxv = 3 * 337000000;
        double score = 0;
        score += (drones[drone->id]->cpTodo - drone->cpTodo) * 337000000;
        score += max(337000000 - drone->getNextCpSquaredD(), 0);
        score = (100 * score) / maxv;
        return score;
    }

    // The higher score the closer to the target
    double getDistanceScore(Drone* drone, int tx, int ty)
    {
        double score = 0;
        score += max(337000000 - drone->getPointSquaredD(tx, ty), 0);
        score = (100 * score) / 337000000;
        return score;
    }

    void launchSimulation()
    {
        for (int turn = 0; turn < SIM_DEPTH; turn++) {
            double turnTime = 0;

            // Application des nouvelles vitesses en fonction des commandes
            preTurnUpdates(turn);

            while (turnTime < 1) {
                double nextColT = 999;
                int cold1 = -1, cold2 = -1;
                for (int did1 = 0; did1 < 4; did1++) {
                    for (int did2 = did1 + 1; did2 < 4; did2++) {
                        if (did1 != did2) {
                            double colTime = testedDrones[did1]->collisionTime(testedDrones[did2]);

                            // Collision occuring before the end of the turn
                            if (turnTime + colTime < 1 && colTime < nextColT) {
                                nextColT = colTime;
                                cold1 = did1;
                                cold2 = did2;
                            }
                        }
                    }
                }

                // Collision occured
                if (cold1 != -1 && cold2 != -1) {
                    // Drones are moved until the collision time
                    for (int did = 0; did < 4; did++) {
                        testedDrones[did]->moveDuring(nextColT);
                    }
                    // Bounce is computed and speeds are updated
                    computeBounce(cold1, cold2);

                    turnTime += nextColT;
                }
                else {
                    break;
                }
            }

            // Drones are moved until the end of the turn
            for (int did = 0; did < 4; did++) {
                testedDrones[did]->moveDuring(1 - turnTime);
            }

            // Trunc/round des vitesses et positions, application de la friction, update de la rotation
            postTurnUpdates(turn);
        }
    }

    /**
     * Ajuste les vitesses des drones suite à la collision en cours entre eux deux
     */
    void computeBounce(int did1, int did2)
    {
        BOUNCE_CALL++;
        Drone* d1 = testedDrones[did1];
        Drone* d2 = testedDrones[did2];

        double d1mass = d1->shieldTimer == SHIELD_TIMER ? 10 : 1;
        double d2mass = d2->shieldTimer == SHIELD_TIMER ? 10 : 1;

        Vector2D normalVector(d1->x - d2->x, d1->y - d2->y);
        normalVector.normalize();
        Vector2D relativeVelocity(d1->speed->dX - d2->speed->dX, d1->speed->dY - d2->speed->dY);
        double dp = normalVector.dotProduct(relativeVelocity);
        normalVector.scale(dp / ((1 / d1mass) + (1 / d2mass)));

        d1->speed->dX -= normalVector.dX * (1 / d1mass);
        d1->speed->dY -= normalVector.dY * (1 / d1mass);
        d2->speed->dX += normalVector.dX * (1 / d2mass);
        d2->speed->dY += normalVector.dY * (1 / d2mass);

        normalVector.scale(max(120 / normalVector.length(), double(1)));

        d1->speed->dX -= normalVector.dX * (1 / d1mass);
        d1->speed->dY -= normalVector.dY * (1 / d1mass);
        d2->speed->dX += normalVector.dX * (1 / d2mass);
        d2->speed->dY += normalVector.dY * (1 / d2mass);
    }

    /**
     * Applique les nouvelles vitesses sans changer les positions
     */
    void preTurnUpdates(int depth)
    {
        for (int did = 0; did < 4; did++) {
            Command* cmd = chromosome->commands[depth * 4 + did];
            Drone* drone = testedDrones[did];

            // Ennemy command
            if (did > 1) {
                if (did == HISRUNNER) {
                    drone->setCheapRaceCmd(cmd);
                } else {
                    drone->setCheapSupportCmd(cmd, testedDrones[MYRUNNER]);
                }
                //cheapPlay(cmd, drone);
            }

            if (cmd->shield) {
                drone->shieldTimer = SHIELD_TIMER;
            }
            else {
                if (drone->shieldTimer > 0) {
                    drone->shieldTimer--;
                }
            }

            if (drone->shieldTimer == 0) {
                drone->speed->dX += precos(drone->rotation + cmd->angle) * cmd->th;
                drone->speed->dY += presin(drone->rotation + cmd->angle) * cmd->th;
            }
        }
    }

    void postTurnUpdates(int depth)
    {
        for (int did = 0; did < 4; did++) {
            Command* cmd = chromosome->commands[depth * 4 + did];
            Drone* drone = testedDrones[did];

            drone->speed->dX *= FRICTION;
            drone->speed->dY *= FRICTION;

            drone->speed->dX = floor(drone->speed->dX);
            if (drone->speed->dX < 0) {
                drone->speed->dX++;
            }
            drone->speed->dY = floor(drone->speed->dY);
            if (drone->speed->dY < 0) {
                drone->speed->dY++;
            }

            drone->x = (int)round(drone->x);
            drone->y = (int)round(drone->y);
            drone->rotation += cmd->angle;

            if (drone->shieldTimer > 0)
                drone->shieldTimer--;

            drone->updateCpState();
        }
    }
};

// ***********************************************************

bool dronePositionsPredicate(Drone* a, Drone* b)
{
    if (a->cpTodo != b->cpTodo) {
        return a->cpTodo < b->cpTodo;
    }
    else {
        return a->getNextCpSquaredD() < b->getNextCpSquaredD();
    }
}

void setDroneRoles()
{
    MYRUNNER = -1;
    MYSUPPORT = -1;
    HISRUNNER = -1;
    HISSUPPORT = -1;

    sort(dronePositions.begin(), dronePositions.end(), dronePositionsPredicate);

    for (int i = 0; i < dronePositions.size(); i++) {
        if (dronePositions[i]->mine) {
            if (MYRUNNER == -1) {
                MYRUNNER = dronePositions[i]->id;
            }
            else {
                MYSUPPORT = dronePositions[i]->id;
            }
        }
        else {
            if (HISRUNNER == -1) {
                HISRUNNER = dronePositions[i]->id;
            }
            else {
                HISSUPPORT = dronePositions[i]->id;
            }
        }
    }

    cerr << "MYRUN:" << MYRUNNER << " MYSUP:" << MYSUPPORT << " HISRUN:" << HISRUNNER << " HISSUP:" << HISSUPPORT << " // MYT:" << MYTIMEOUT << " HIST:" << HISTIMEOUT << endl;
}

void cheapPlay(Command* cmd, Drone* drone)
{
    Checkpoint* cp;
    if (drone->nextCp != -1) {
        cp = checkPoints[drone->nextCp];
    }
    else {
        cp = checkPoints[0];
    }
    double ncpd = drone->getNextCpSquaredD();
    double speed = drone->speed->length();
    if (ncpd < 1500 * 1500 && speed > 250 && drone->nextNextCp != -1) {
        drone->setCPCmd(cmd, checkPoints[drone->nextNextCp], 25);
    }
    else if (ncpd > 4000 * 4000) {
        drone->setCPCmd(cmd, cp, 100);
    }
    else {
        drone->setCPCmd(cmd, cp, 40);
    }
}

void initGeneticPopulation(array<Chromosome*, GEN_POP_SIZE> population)
{
    Command* dummy0 = new Command(0, 0, false);
    cheapPlay(dummy0, drones[0]);
    Command* dummy1 = new Command(0, 0, false);
    cheapPlay(dummy1, drones[1]);

    // 3/4, des petites mutations sur les commandes dummy
    for (int chromId = 0; chromId < (3 * GEN_POP_SIZE) / 4; chromId++) {
        Chromosome* ch = population[chromId];
        ch->score = 0;
        for (int level = 0; level < SIM_DEPTH; level++) {
            ch->commands[level * 4]->copy(dummy0);
            ch->commands[level * 4 + 1]->copy(dummy1);
            if (level < 3) { // On ne mute que les premiers tours
                ch->commands[level * 4]->smallMutation(level == 0);
                ch->commands[level * 4 + 1]->smallMutation(level == 0);
            }
            else {
                ch->commands[level * 4]->randomize(level == 0);
                ch->commands[level * 4 + 1]->randomize(level == 0);
            }
        }
    }

    // 1/4, full random
    for (int chromId = (3 * GEN_POP_SIZE) / 4; chromId < GEN_POP_SIZE; chromId++) {
        Chromosome* ch = population[chromId];
        ch->score = 0;
        for (int level = 0; level < SIM_DEPTH; level++) {
            ch->commands[level * 4]->randomize(level == 0);
            ch->commands[level * 4 + 1]->randomize(level == 0);
            // ch->commands[level * 4 + 2] = NULL;
            // ch->commands[level * 4 + 3] = NULL;
        }
    }

    my_delete(dummy0);
    my_delete(dummy1);
}

bool chromScorePredicate(Chromosome* c1, Chromosome* c2)
{
    return c1->score > c2->score;
}

void getNextGeneticPopulation(array<Chromosome*, GEN_POP_SIZE> oldPopulation, array<Chromosome*, GEN_POP_SIZE> newPopulation)
{
    // Tri de la population, les premiers sont les meilleurs
    sort(oldPopulation.begin(), oldPopulation.end(), chromScorePredicate);

    /**
     * Croisement entre deux chromosomes = 1 swap de commande
     * Mutation d'un chromosome = 1 mutation légère sur une commande choisie aléatoirement
     *
     * 1/2 : croisements entre paire de chromosomes issue du top 50%
     * 3/8 : mutations aléatoires sur chromosomes issus du top 50%
     * 1/8 : full random
     */

    // Ajoute des paires de chromosomes dans la liste
    for (int i = 0; i < GEN_POP_SIZE / 2; i += 2) {
        Chromosome* ch1 = oldPopulation[randInt(0, GEN_POP_SIZE / 2)];
        Chromosome* ch2 = oldPopulation[randInt(0, GEN_POP_SIZE / 2)];
        Chromosome* nch1 = newPopulation[i];
        nch1->copy(ch1);
        nch1->score = 0;
        Chromosome* nch2 = newPopulation[i + 1];
        nch2->copy(ch2);
        nch2->score = 0;

        int crossLevel = randInt(0, SIM_DEPTH);
        int crossDid = randInt(0, 2);
        Command* tmp = nch1->commands[crossLevel * 4 + crossDid];
        nch1->commands[crossLevel * 4 + crossDid] = nch2->commands[crossLevel * 4 + crossDid];
        nch2->commands[crossLevel * 4 + crossDid] = tmp;

        crossLevel = randInt(0, SIM_DEPTH);
        crossDid = randInt(0, 2);
        tmp = nch1->commands[crossLevel * 4 + crossDid];
        nch1->commands[crossLevel * 4 + crossDid] = nch2->commands[crossLevel * 4 + crossDid];
        nch2->commands[crossLevel * 4 + crossDid] = tmp;
    }

    for (int i = GEN_POP_SIZE / 2; i < ((15 * GEN_POP_SIZE) / 16); i++) {
        Chromosome* ch = oldPopulation[randInt(0, GEN_POP_SIZE / 2)];
        Chromosome* nch = newPopulation[i];
        nch->copy(ch);
        nch->score = 0;
        int crossLevel = randInt(0, SIM_DEPTH);
        int crossDid = randInt(0, 2);
        nch->commands[crossLevel * 4 + crossDid]->smallMutation(crossLevel == 0);

        crossLevel = randInt(0, SIM_DEPTH);
        crossDid = randInt(0, 2);
        nch->commands[crossLevel * 4 + crossDid]->smallMutation(crossLevel == 0);
    }

    for (int i = ((15 * GEN_POP_SIZE) / 16); i < GEN_POP_SIZE; i++) {
        Chromosome* ch = newPopulation[i];
        ch->score = 0;
        for (int level = 0; level < SIM_DEPTH; level++) {
            ch->commands[level * 4]->randomize(level == 0);
            ch->commands[level * 4 + 1]->randomize(level == 0);
            // ch->commands[level * 4 + 2] = NULL;
            // ch->commands[level * 4 + 3] = NULL;
        }
    }
}

void play(Command* cmd0, Command* cmd1)
{
    int geneticBatch = 0;
    double bestScore = -9999999;
    int bestScoreBatchN = 0;

    array<Chromosome*, GEN_POP_SIZE> pop;
    array<Chromosome*, GEN_POP_SIZE> oldPop;
    for (int i = 0; i < GEN_POP_SIZE; i++) {
        pop[i] = new Chromosome();
        oldPop[i] = new Chromosome();
    }
    bool popInit = false;

    // SIM_BRAN < 20 * populationSize ||
    while (elapsedMS() < 145) { // TODO : augmenter ?
        if (!popInit) {
            initGeneticPopulation(pop);
            popInit = true;
        }
        else {
            getNextGeneticPopulation(oldPop, pop);
        }

        for (int chromId = 0; chromId < GEN_POP_SIZE; chromId++) {
            Chromosome* chromosome = pop[chromId];
            Simulator sim(chromosome);
            sim.launchSimulation();
            double score = sim.evaluate();
            if (chromosome->shieldUser()) {
                score -= 2;
            }
            chromosome->score = score;

            if (score > bestScore) {
                bestScore = score;
                bestScoreBatchN = geneticBatch;
                cmd0->copy(chromosome->commands[0]);
                cmd1->copy(chromosome->commands[1]);
            }

            SIM_BRAN++;
        }
        swap(pop, oldPop); // TODO swap toute la liste et pas tous les pointeurs un par un
        geneticBatch++;
    }

    for (int i = 0; i < GEN_POP_SIZE; i++) {
        delete pop[i];
        delete oldPop[i];
    }

    cerr << "Best score = " << bestScore << " from batch n." << bestScoreBatchN << "/" << (geneticBatch - 1) << endl;
}

void choseStrategy()
{
    if (MYTIMEOUT < 30) {
        STRATEGY = 4;
    }
    else if (!dronePositions[0]->mine && drones[MYRUNNER]->cpTodo - drones[HISRUNNER]->cpTodo > 4 && MYTIMEOUT > HISTIMEOUT) {
        // Je suis deuxieme, j'ai au moins 4 CP de retard sur le premier et j'ai un timeout plus grand que lui ..
        STRATEGY = 1; // YOLO
    }
    else {
        Vector2D v(drones[MYSUPPORT]->speed);
        v.add(drones[HISRUNNER]->speed);

        if (v.length() > drones[HISRUNNER]->speed->length()) {
            STRATEGY = 3;
        }
        else {
            STRATEGY = 2;
        }
    }
}

void outputInstruction(Command* cmd, Drone* drone)
{
    int tx = (int)(drone->x + precos(drone->rotation + cmd->angle) * 400);
    int ty = (int)(drone->y + presin(drone->rotation + cmd->angle) * 400);
    cout << tx << " " << ty << " " << (cmd->shield ? "SHIELD" : cmd->th == 650 ? "BOOST" : to_string(cmd->th)) << endl;
}

int main()
{
    cin >> laps;
    cin.ignore();
    cin >> checkpointCount;
    cin.ignore();

    totalCp = laps * checkpointCount;
    for (int i = 0; i < 4; i++) {
        drones.push_back(new Drone(i));
        drones[i]->cpTodo = laps * checkpointCount;
        dronePositions.push_back(drones[i]);
    }

    for (int i = 0; i < checkpointCount; i++) {
        int checkpointX;
        int checkpointY;
        cin >> checkpointX >> checkpointY;
        checkPoints.push_back(new Checkpoint(checkpointX, checkpointY));
        cin.ignore();
    }
    precalculateCosSin();

    // game loop
    while (1) {
        cpt = 0;
        BOUNCE_CALL = 0;
        MOVDUR_CALL = 0;
        SIM_BRAN = 0;
        for (int i = 0; i < 4; i++) {
            int x;
            int y;
            int vx;
            int vy;
            int angle;
            int nextCheckPointId;
            cin >> x >> y >> vx >> vy >> angle >> nextCheckPointId;
            cin.ignore();

            Drone* drone = drones[i];
            drone->x = x;
            drone->y = y;
            drone->speed->dX = vx;
            drone->speed->dY = vy;
            drone->rotation = angle;
            int oldncp = drone->nextCp;
            drone->nextCp = nextCheckPointId;
            if (oldncp != -1 && drone->nextCp != oldncp) {
                drone->cpTodo--;
                if (drone->mine) {
                    MYTIMEOUT = 100;
                }
                else {
                    HISTIMEOUT = 100;
                }
            }
            drone->nextNextCp = drone->cpTodo == 1 ? -1 : (drone->nextCp + 1) % checkpointCount;
            if (drone->shieldTimer > 0) {
                drone->shieldTimer--;
            }
        }

        MYTIMEOUT--;
        HISTIMEOUT--;

        gettimeofday(&timestart, NULL);
        setDroneRoles();
        choseStrategy();

        Command* cmd0 = new Command(0, 0, false);
        Command* cmd1 = new Command(0, 0, false);
        if (turn == 0) {
            drones[0]->setNextCPCmd(cmd0, 650);
            drones[1]->setNextCPCmd(cmd1, 650);
        }
        else {
            play(cmd0, cmd1);
        }

        float elapsed = elapsedMS();
        cerr << "turn in " << elapsed << " ms. Strategy " << STRATEGY << endl;
        cerr << SIM_BRAN << " simulated branches with BOUNCE_CALL:" << BOUNCE_CALL << " MOVDUR_CALL:" << MOVDUR_CALL << endl;

        if (cmd0->shield) {
            drones[0]->shieldTimer = SHIELD_TIMER + 1;
        }
        if (cmd1->shield) {
            drones[1]->shieldTimer = SHIELD_TIMER + 1;
        }
        outputInstruction(cmd0, drones[0]);
        outputInstruction(cmd1, drones[1]);
        my_delete(cmd0);
        my_delete(cmd1);
        turn++;
    }
}