[Tryout] 2016 FB magus code usage for Sogeti contest

//#pragma GCC optimize("-O3")
#pragma GCC optimize("-O0")
#pragma GCC optimize("inline")
#pragma GCC optimize("omit-frame-pointer")
#pragma GCC optimize("unroll-loops")

#include <iostream>
#include <chrono>
#include <string>
#include <algorithm>
#include <math.h>

using namespace std;
using namespace std::chrono;

#define PROFILE
#define PROD
// #define DEBUG

#ifdef PROFILE
const int DURATIONS_COUNT = 1;
    double durations[DURATIONS_COUNT];
    high_resolution_clock::time_point starts[DURATIONS_COUNT];

#define PS(i)   starts[i] = NOW;
#define PE(i)   durations[i] = durations[i] + duration_cast<duration<double>>(NOW - starts[i]).count();
#else
#define PS(i)
#define PE(i)
#endif

high_resolution_clock::time_point start;
#define NOW high_resolution_clock::now()
#define TIME duration_cast<duration<double>>(NOW - start).count()

// ***********************************************************
constexpr double WIDTH = 16000.0;
constexpr double HEIGHT = 7500.0;

constexpr double E = 0.00001;
constexpr int VERTICAL = 1;
constexpr int HORIZONTAL = 2;
constexpr double INF = 16000 * 16000 + 7500 * 7500;
constexpr int WIZARD = 1;
constexpr int SNAFFLE = 2;
constexpr int BLUDGER = 3;
constexpr int POLE = 4;
constexpr double TO_RAD = M_PI / 180.0;

constexpr double ANGLES[] = {0.0, 10.0, 20.0, 30.0, 40.0, 50.0, 60.0, 70.0, 80.0, 90.0, 100.0, 110.0, 120.0, 130.0,
                             140.0, 150.0, 160.0, 170.0, 180.0, 190.0, 200.0, 210.0, 220.0, 230.0, 240.0, 250.0, 260.0,
                             270.0, 280.0, 290.0, 300.0, 310.0, 320.0, 330.0, 340.0, 350.0};
constexpr int ANGLES_LENGTH = 36;

constexpr int DEPTH = 4;
constexpr int SPELL_DEPTH = 8;
constexpr int POOL = 50;
constexpr double MUTATION = 2;

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

double cosAngles[ANGLES_LENGTH];
double sinAngles[ANGLES_LENGTH];

int myTeam;
int mana = 0;
int turn = 0;
int myScore = 0;
int hisScore = 0;
double energy = 0;
int depth = 0;

int smana;
int smyScore;
int shisScore;

class Point;

class Unit;

class Wizard;

class Snaffle;

class Bludger;

class Spell;

class Pole;

Wizard *myWizard1;
Wizard *myWizard2;
Wizard *hisWizard1;
Wizard *hisWizard2;
Point *myGoal;
Point *hisGoal;
Point *mid;

Unit *units[20];
int unitsFE = 0;

Unit *unitsById[24];

Snaffle *snaffles[20];
int snafflesFE = 0;

Wizard *wizards[4];
Bludger *bludgers[2];
Pole *poles[4];

Unit *spellTargets[20];
int spellTargetsFE;

bool doLog = false;

Spell *spells[4];

int victory;

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

static unsigned int g_seed;

inline void fast_srand(int seed) {
    //Seed the generator
    g_seed = seed;
}

inline int fastrand() {
    //fastrand routine returns one integer, similar output value range as C lib.
    g_seed = (214013 * g_seed + 2531011);
    return (g_seed >> 16) & 0x7FFF;
}

inline int fastRandInt(int maxSize) {
    return fastrand() % maxSize;
}

inline int fastRandInt(int a, int b) {
    return (a + fastRandInt(b - a));
}

inline double fastRandDouble() {
    return static_cast<double>(fastrand()) / 0x7FFF;
}

inline double fastRandDouble(double a, double b) {
    return a + (static_cast<double>(fastrand()) / 0x7FFF) * (b - a);
}

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

class Solution {
public:
    double energy;
    int moves1[DEPTH];
    int moves2[DEPTH];

    int spellTurn1;
    Unit *spellTarget1;
    int spell1;

    int spellTurn2;
    Unit *spellTarget2;
    int spell2;

    Solution *merge(Solution *solution) {
        Solution *child = new Solution();

        for (int i = 0; i < DEPTH; ++i) {
            if (fastRandInt(2)) {
                child->moves1[i] = solution->moves1[i];
                child->moves2[i] = solution->moves2[i];
            } else {
                child->moves1[i] = moves1[i];
                child->moves2[i] = moves2[i];
            }
        }

        // TEST without Spells
/*        if (fastRandInt(2)) {
            child->spellTurn1 = solution->spellTurn1;
            child->spellTarget1 = solution->spellTarget1;
        } else {
            child->spellTurn1 = spellTurn1;
            child->spellTarget1 = spellTarget1;
        }

        if (fastRandInt(2)) {
            child->spellTurn2 = solution->spellTurn2;
            child->spellTarget2 = solution->spellTarget2;
        } else {
            child->spellTurn2 = spellTurn2;
            child->spellTarget2 = spellTarget2;
        }*/

        return child;
    }

    void copy(Solution *solution) {
        for (int i = 0; i < DEPTH; ++i) {
            moves1[i] = solution->moves1[i];
            moves2[i] = solution->moves2[i];
        }

        // TEST without Spells
/*
        spellTurn1 = solution->spellTurn1;
        spellTarget1 = solution->spellTarget1;
        spellTurn2 = solution->spellTurn2;
        spellTarget2 = solution->spellTarget2;*/

        this->energy = solution->energy;
    }

    void mutate();

    void randomize();

};



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

class Point {
public:
    double x;
    double y;

    Point() {};

    Point(double x, double y) {
        this->x = x;
        this->y = y;
    }

};

inline double dist(double x1, double y1, double x2, double y2) {
    return sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
}

inline double dist2(double x1, double y1, double x2, double y2) {
    return (x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2);
}

inline double dist(Point *p, double x2, double y2) {
    return dist(p->x, p->y, x2, y2);
}

inline double dist2(Point *p, double x2, double y2) {
    return dist2(p->x, p->y, x2, y2);
}

inline double dist(Point *u1, Point *u2) {
    return dist(u1->x, u1->y, u2->x, u2->y);
}

inline double dist2(Point *u1, Point *u2) {
    return dist2(u1->x, u1->y, u2->x, u2->y);
}

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

class Collision {
public:
    double t;
    int dir;
    Unit *a;
    Unit *b;

    Collision() {}

    Collision *update(double t, Unit *a, int dir) {
        b = NULL;
        this->t = t;
        this->a = a;
        this->dir = dir;

        return this;
    }

    Collision *update(double t, Unit *a, Unit *b) {
        dir = 0;
        this->t = t;
        this->a = a;
        this->b = b;

        return this;
    }
};

Collision **collisionsCache;
int collisionsCacheFE = 0;

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

class Spell {
public:
    Wizard *caster;
    Unit *target;

    Unit *starget;

    void cast(Unit *target);

    void apply();

    void save();

    void reset() {
        target = starget;
    }

    void reloadTarget();

    virtual void effect() {}

    virtual void print() {}
};

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

class Wingardium : public Spell {
public:
    Wingardium(Wizard *caster) {
        this->caster = caster;
    }

    virtual void print();

    virtual void effect();
};

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

class Unit : public Point {
public:
    int id;
    int type;
    int state;
    bool dead = false;
    double r;
    double m;
    double f;
    double vx;
    double vy;

    double sx;
    double sy;
    double svx;
    double svy;

    Wizard *carrier = NULL;
    Snaffle *snaffle = NULL;

    int grab = 0;

#ifndef PROD
    double lx;
    double ly;
    double lvx;
    double lvy;
    Wizard* lcarrier;
    Snaffle* lsnaffle;
    bool ldead;

    void store() {
        lx = x;
        ly = y;
        lvx = vx;
        lvy = vy;
        lcarrier = carrier;
        lsnaffle = snaffle;
        ldead = dead;
    }

    void compare();
#endif

    void update(int id, int x, int y, int vx, int vy, int state) {
        this->id = id;
        this->x = x;
        this->y = y;
        this->vx = vx;
        this->vy = vy;
        this->state = state;
    }

    double speedTo(Point *p) {
        double d = 1.0 / dist(this, x, y);

        // vitesse dans la direction du checkpoint - (vitesse orthogonale)^2/dist au cheeckpoint
        double dx = (p->x - this->x) * d;
        double dy = (p->y - this->y) * d;
        double nspeed = vx * dx + vy * dy;
        double ospeed = dy * vx - dx * vy;

        return nspeed - (5 * ospeed * ospeed * d);
    }

    double speed() {
        return sqrt(vx * vx + vy * vy);
    }

    inline void thrust(double thrust, Point *p, double distance) {
        double coef = (thrust / m) / distance;
        vx += (p->x - x) * coef;
        vy += (p->y - y) * coef;
    }

    inline void thrust(double thrust, double x, double y, double distance) {
        double coef = (thrust / m) / distance;
        vx += (x - this->x) * coef;
        vy += (y - this->y) * coef;
    }

    virtual void move(double t) {
        x += vx * t;
        y += vy * t;
    }

    virtual Collision *collision(double from) {
        double tx = 2.0;
        double ty = tx;

        if (x + vx < r) {
            tx = (r - x) / vx;
        } else if (x + vx > WIDTH - r) {
            tx = (WIDTH - r - x) / vx;
        }

        if (y + vy < r) {
            ty = (r - y) / vy;
        } else if (y + vy > HEIGHT - r) {
            ty = (HEIGHT - r - y) / vy;
        }

        int dir;
        double t;

        if (tx < ty) {
            dir = HORIZONTAL;
            t = tx + from;
        } else {
            dir = VERTICAL;
            t = ty + from;
        }

        if (t <= 0.0 || t > 1.0) {
            return NULL;
        }

        return collisionsCache[collisionsCacheFE++]->update(t, this, dir);
    }

    virtual Collision *collision(Unit *u, double from) {
        double x2 = x - u->x;
        double y2 = y - u->y;
        double r2 = r + u->r;
        double vx2 = vx - u->vx;
        double vy2 = vy - u->vy;
        double a = vx2 * vx2 + vy2 * vy2;

        if (a < E) {
            return NULL;
        }

        double b = -2.0 * (x2 * vx2 + y2 * vy2);
        double delta = b * b - 4.0 * a * (x2 * x2 + y2 * y2 - r2 * r2);

        if (delta < 0.0) {
            return NULL;
        }

        // double sqrtDelta = sqrt(delta);
        // double d = 1.0/(2.0*a);
        // double t1 = (b + sqrtDelta)*d;
        // double t2 = (b - sqrtDelta)*d;
        // double t = t1 < t2 ? t1 : t2;

        double t = (b - sqrt(delta)) * (1.0 / (2.0 * a));

        if (t <= 0.0) {
            return NULL;
        }

        t += from;

        if (t > 1.0) {
            return NULL;
        }

        return collisionsCache[collisionsCacheFE++]->update(t, this, u);
    }

    virtual void bounce(Unit *u) {
        double mcoeff = (m + u->m) / (m * u->m);
        double nx = x - u->x;
        double ny = y - u->y;
        double nxnydeux = nx * nx + ny * ny;
        double dvx = vx - u->vx;
        double dvy = vy - u->vy;
        double product = (nx * dvx + ny * dvy) / (nxnydeux * mcoeff);
        double fx = nx * product;
        double fy = ny * product;
        double m1c = 1.0 / m;
        double m2c = 1.0 / u->m;

        vx -= fx * m1c;
        vy -= fy * m1c;
        u->vx += fx * m2c;
        u->vy += fy * m2c;

        // Normalize vector at 100
        double impulse = sqrt(fx * fx + fy * fy);
        if (impulse < 100.0) {
            double min = 100.0 / impulse;
            fx = fx * min;
            fy = fy * min;
        }

        vx -= fx * m1c;
        vy -= fy * m1c;
        u->vx += fx * m2c;
        u->vy += fy * m2c;
    }

    virtual void bounce(int dir) {
        if (dir == HORIZONTAL) {
            vx = -vx;
        } else {
            vy = -vy;
        }
    }

    virtual void end() {
        x = round(x);
        y = round(y);
        vx = round(vx * f);
        vy = round(vy * f);
    }

    bool can(Unit *u) {
        if (type == SNAFFLE) {
            return !carrier && !dead && !u->snaffle && !u->grab;
        } else if (u->type == SNAFFLE) {
            return !u->carrier && !u->dead && !snaffle && !grab;
        }

        return true;
    }

    virtual void print() {}

    virtual void save() {
        sx = x;
        sy = y;
        svx = vx;
        svy = vy;
    }

    virtual void reset() {
        x = sx;
        y = sy;
        vx = svx;
        vy = svy;
    }
};

// ****************************************************************************************
class Pole : public Unit {
public:

    Pole(int id, double x, double y) {
        this->id = id;
        r = 300;
        m = INF;
        type = POLE;
        this->x = x;
        this->y = y;
        vx = 0;
        vy = 0;
        dead = false;
        f = 0.0;
    }

    virtual void move(double t) {}

    virtual void save() {}

    virtual void reset() {}

    virtual Collision *collision(double from) {
        return NULL;
    }
};

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

class Wizard : public Unit {
public:
    int team;
    Spell *spell;

    int sgrab;
    Snaffle *ssnaffle;

    Unit *spellTarget;

    Wizard(int team) {
        this->team = team;
        this->r = 400.0;
        this->m = 1;
        this->f = 0.75;

        snaffle = NULL;
        grab = 0;
        type = WIZARD;

        spell = new Wingardium(this);

        spellTarget = NULL;
    }

    inline void grabSnaffle(Snaffle *snaffle);

    void apply(int move);

    void output(int move, int spellTurn, Unit *target) {
        spellTurn = true; // added for debug
        if (!spellTurn) {
            cout << "WINGARDIUM " << target->id << " " << myGoal->x << " " << myGoal->y << endl;
            return;
        }

        // Adjust the targeted point for this angle
        // Find a point with the good angle
        double px = x + cosAngles[move] * 10000.0;
        double py = y + sinAngles[move] * 10000.0;

        if (snaffle) {
            cout << "THROW " << round(px) << " " << round(py) << " 500";
        } else {
            cout << "MOVE " << round(px) << " " << round(py) << " 150";
        }

        cout << endl;
        // cerr << "output wizard team #" << team << endl;
    }

    bool cast(Unit *target) {
        if (target->dead) {
            return false;
        }

        spellTarget = target;

        return true;
    }

    virtual Collision *collision(Unit *u, double from) {
        if (u->type == SNAFFLE) {
            u->r = -1.0;
            Collision *result = Unit::collision(u, from);
            u->r = 150.0;

            return result;
        } else {
            return Unit::collision(u, from);
        }
    }

    virtual void bounce(Unit *u);

    void play();

    virtual void end();

    virtual void save() {
        Unit::save();

        sgrab = grab;
        ssnaffle = snaffle;
    }

    virtual void reset() {
        Unit::reset();

        grab = sgrab;
        snaffle = ssnaffle;
    }

    virtual void print();

    void updateSnaffle();

    void apply(Solution *solution, int turn, int index) {
        if (index == 1) {
            if (solution->spellTurn1 == turn) {
                if (!myWizard1->cast(solution->spellTarget1)) {
                    myWizard1->apply(solution->moves1[turn]);
                }
            } else {
                myWizard1->apply(solution->moves1[turn]);
            }
        } else {
            if (solution->spellTurn2 == turn) {
                if (!myWizard2->cast(solution->spellTarget2)) {
                    myWizard2->apply(solution->moves2[turn]);
                }
            } else {
                myWizard2->apply(solution->moves2[turn]);
            }
        }
    }
};

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

class Snaffle : public Unit {
public:
    Wizard *scarrier;


    Snaffle() {
        this->r = 150.0;
        this->m = 0.5;
        this->f = 0.75;

        carrier = NULL;
        type = SNAFFLE;
    }

    virtual Collision *collision(double from) {
        if (carrier || dead) {
            return NULL;
        }

        double tx = 2.0;
        double ty = tx;

        if (x + vx < 0.0) {
            tx = -x / vx;
        } else if (x + vx > WIDTH) {
            tx = (WIDTH - x) / vx;
        }

        if (y + vy < r) {
            ty = (r - y) / vy;
        } else if (y + vy > HEIGHT - r) {
            ty = (HEIGHT - r - y) / vy;
        }

        int dir;
        double t;

        if (tx < ty) {
            dir = HORIZONTAL;
            t = tx + from;
        } else {
            dir = VERTICAL;
            t = ty + from;
        }

        if (t <= 0.0 || t > 1.0) {
            return NULL;
        }

        return collisionsCache[collisionsCacheFE++]->update(t, this, dir);
    }

    virtual Collision *collision(Unit *u, double from) {
        if (u->type == WIZARD) {
            r = -1.0;
            Collision *result = Unit::collision(u, from);
            r = 150.0;

            return result;
        } else {
            return Unit::collision(u, from);
        }
    }

    virtual void bounce(Unit *u) {
        if (u->type == WIZARD) {
            Wizard *target = (Wizard *) u;
            if (!target->snaffle && !target->grab && !dead && !carrier) {
                target->grabSnaffle(this);
            }
        } else {
            Unit::bounce(u);
        }
    }

    virtual void bounce(int dir) {
        if (dir == HORIZONTAL && y >= 1899.0 && y <= 5599.0) {
            dead = true;

            if (!myTeam) {
                if (x > 8000) {
                    myScore += 1;
                } else {
                    hisScore += 1;
                }
            } else {
                if (x > 8000) {
                    hisScore += 1;
                } else {
                    myScore += 1;
                }
            }
        } else {
            Unit::bounce(dir);
        }
    }

    virtual void move(double t) {
        if (!dead && !carrier) {
            Unit::move(t);
        }
    }

    virtual void end() {
        if (!dead && !carrier) {
            Unit::end();
        }
    }

    virtual void save() {
        Unit::save();

        scarrier = carrier;
    }

    virtual void reset() {
        Unit::reset();

        carrier = scarrier;
        dead = false;
    }

    virtual void print() {
        if (dead) {
            cerr << "Snaffle " << id << " dead";
        } else {
            cerr << "Snaffle " << id << " " << x << " " << y << " " << vx << " " << vy << " " << speed() << " "
                 << " | ";

            if (carrier) {
                cerr << "Carrier " << carrier->id << " | ";
            }
        }

        cerr << endl;
    }
};

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

class Bludger : public Unit {
public:
    Wizard *last;
    int ignore[2];

    Wizard *slast;

    Bludger() {
        this->r = 200.0;
        this->m = 8;
        this->f = 0.9;

        last = NULL;
        ignore[0] = -1;
        ignore[1] = -1;
        type = BLUDGER;
    }

    virtual void print() {
        cerr << "Bludger " << id << " " << x << " " << y << " " << vx << " " << vy << " " << speed() << " " << ignore[0]
             << " " << ignore[1] << " | ";

        if (last) {
            cerr << "Last " << last->id << " | ";
        }

        cerr << endl;
    }

    virtual void save() {
        Unit::save();

        slast = last;
    }

    virtual void reset() {
        Unit::reset();

        last = slast;
        ignore[0] = -1;
        ignore[1] = -1;
    }

    virtual void bounce(Unit *u) {
        if (u->type == WIZARD) {
            last = (Wizard *) u;
        }

        Unit::bounce(u);
    }

    void play() {
        // Find our target
        Wizard *target = NULL;
        double d;

        for (int i = 0; i < 4; ++i) {
            Wizard *wizard = wizards[i];

            if ((last && last->id == wizard->id) || wizard->team == ignore[0] || wizard->team == ignore[1]) {
                continue;
            }

            double d2 = dist2(this, wizard);

            if (!target || d2 < d) {
                d = d2;
                target = wizard;
            }
        }

        if (target) {
            thrust(1000.0, target, sqrt(d));
        }

        ignore[0] = -1;
        ignore[1] = -1;
    }
};

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

void Wingardium::print() {
    if (target != NULL)
        cerr << "Wingardium " << target->id << endl;
    else
        cerr << "No target defined for spell Windardium!! " << endl;
}

void Spell::cast(Unit *target) {
    this->target = target;
}

void Spell::apply() {
    if (!target->dead)
        effect();
}

void Spell::reloadTarget() {
    if (target->dead) {
        // Cancel the spell
        target = NULL;
    }
}

void Spell::save() {
    starget = target;
}

void Wingardium::effect() {
    ((Bludger *) target)->ignore[caster->team] = caster->team;
}
/*
void Petrificus::effect() {
    target->vx = 0.0;
    target->vy = 0.0;
}

void Accio::effect() {
    double d = dist(caster, target);

    if (d < 10.0) {
        return;
    }

    double dcoef = d*0.001;
    double power = 3000.0 / (dcoef*dcoef);

    if (power > 1000.0) {
        power = 1000.0;
    }

    dcoef = 1.0 / d;
    power = power / target->m;
    target->vx -= dcoef * power * (target->x - caster->x);
    target->vy -= dcoef * power * (target->y - caster->y);
}

void Flipendo::effect() {
    double d = dist(caster, target);

    if (d < 10.0) {
        return;
    }

    double dcoef = d*0.001;
    double power = 6000.0 / (dcoef*dcoef);

    if (power > 1000.0) {
        power = 1000.0;
    }

    dcoef = 1.0 / d;
    power = power / target->m;
    target->vx += dcoef * power * (target->x - caster->x);
    target->vy += dcoef * power * (target->y - caster->y);
}*/

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

#ifndef PROD
void Unit::compare() {
    if (lx != x || ly != y || lvx != vx || lvy != vy || lcarrier != carrier || lsnaffle != snaffle || ldead != dead) {
        cerr << "Diff " << id << " : "
             << (x - lx) << " "
             << (y - ly) << " "
             << dist(x, y, lx, ly) << " | "
             << (vx - lvx) << " "
             << (vy - lvy) << " "
             << (speed() - sqrt(lvx*lvx + lvy*lvy)) << " | "
             << carrier << " " << lcarrier << " | "
             << snaffle << " " << lsnaffle << " | ";

        if (ldead) {
            cerr << "dead";
        }

        cerr << endl;
    }
}
#endif

void Wizard::print() {
    cerr << "Wizard " << id << " " << x << " " << y << " " << vx << " " << vy << " " << speed() << " " << grab << " | ";

    if (snaffle) {
        cerr << "Snaffle " << snaffle->id << " | ";
    }

//    for (int i = 0; i < 4; ++i) {
//        spells[i]->print();
//    }

    cerr << endl;
}

void Wizard::updateSnaffle() {
    if (state) {
        for (int i = 0; i < snafflesFE; ++i) {
            Snaffle *snaffle = snaffles[i];

            if (snaffle->x == x && snaffle->y == y && snaffle->vx == vx && snaffle->vy == vy) {
                this->snaffle = snaffle;
                snaffle->carrier = this;
            }
        }

        grab = 3;
    } else {
        if (grab) {
            grab -= 1;
        }
        snaffle = NULL;
    }
}

void Wizard::play() {
    // Relacher le snaffle qu'on porte dans tous les cas
    if (snaffle) {
        snaffle->carrier = NULL;
        snaffle = NULL;
    }
}

void Wizard::end() {
    Unit::end();

    if (grab) {
        grab -= 1;

        if (!grab) {
            // Check if we can grab a snaffle
            for (int i = 0; i < snafflesFE; ++i) {
                Snaffle *snaffle = snaffles[i];

                if (!snaffle->dead && !snaffle->carrier && dist2(this, snaffle) < 159201.0) {
                    grabSnaffle(snaffle);
                    break;
                }
            }
        }
    }

    if (snaffle) {
        snaffle->x = x;
        snaffle->y = y;
        snaffle->vx = vx;
        snaffle->vy = vy;
    }

    if (spellTarget) {
        spell->cast(spellTarget);
        spellTarget = NULL;
    }
}

void Wizard::bounce(Unit *u) {
    if (u->type == SNAFFLE) {
        Snaffle *target = (Snaffle *) u;
        if (!snaffle && !grab && !target->dead && !target->carrier) {
            grabSnaffle(target);
        }
    } else {
        if (u->type == BLUDGER) {
            ((Bludger *) u)->last = this;
        }

        Unit::bounce(u);
    }
}

inline void Wizard::grabSnaffle(Snaffle *snaffle) {
    grab = 4;
    snaffle->carrier = this;
    this->snaffle = snaffle;

}

void Wizard::apply(int move) {
#ifdef DEBUG
    cerr << "DEBUG 5 : Wizard::apply" <<endl;
#endif
    if (snaffle) {
        double coef = 500.0 * (1.0 / snaffle->m);
        snaffle->vx += cosAngles[move] * coef;
        snaffle->vy += sinAngles[move] * coef;
    } else {
        vx += cosAngles[move] * 150.0;
        vy += sinAngles[move] * 150.0;
    }
}

void Solution::mutate() {
    // TEST WITHOUT SPELLS
    int r = fastRandInt(2);
//    int r = fastRandInt(4);

    if (!r) {
        // Change a moves1
        moves1[fastRandInt(DEPTH)] = fastRandInt(ANGLES_LENGTH);
    } else if (r == 1) {
        // Change a moves2
        moves2[fastRandInt(DEPTH)] = fastRandInt(ANGLES_LENGTH);
    } else if (r == 2) {
        // Change spell1
        spellTurn1 = fastRandInt(SPELL_DEPTH);
        spellTarget1 = spellTargets[fastRandInt(spellTargetsFE)];
    } else {
        // Change spell2
        spellTurn2 = fastRandInt(SPELL_DEPTH);
        spellTarget2 = spellTargets[fastRandInt(spellTargetsFE)];
        spellTarget2->speed();
    }

}

void Solution::randomize() {
    for (int i = 0; i < DEPTH; ++i) {
        moves1[i] = fastRandInt(ANGLES_LENGTH);
        moves2[i] = fastRandInt(ANGLES_LENGTH);
//        cerr << "moves1 -> " << moves1[i] <<endl;
//        cerr << "moves2 -> " << moves2[i] <<endl;
    }

    // TEST WITHOUT SPELLS
/*    spellTurn1 = fastRandInt(SPELL_DEPTH);
    spellTarget1 = spellTargets[fastRandInt(spellTargetsFE)];
    spellTurn2 = fastRandInt(SPELL_DEPTH);
    spellTarget2 = spellTargets[fastRandInt(spellTargetsFE)];*/

}

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

bool mustErase(Collision *col, Unit *a, Unit *b) {
    if (a->id == col->a->id) {
        return true;
    }

    if (b != NULL && col->b != NULL) {
        if (a->id == col->b->id
            || b->id == col->a->id
            || b->id == col->b->id) {
            return true;
        }
    } else if (b != NULL) {
        if (b->id == col->a->id) {
            return true;
        }
    } else if (col->b != NULL) {
        if (a->id == col->b->id) {
            return true;
        }
    }

    return false;
}

Collision **collisions;
int collisionsFE = 0;

Collision **tempCollisions;
int tempCollisionsFE = 0;

Collision *fake;

void move() {
    double t = 0.0;
    double delta;

    Collision *next = fake;
    collisionsCacheFE = 0;
    collisionsFE = 0;
    tempCollisionsFE = 0;

    Collision *col;
    Unit *a;
    Unit *b;
    Unit *u;
    int i, j;

    // Get first collisions
    for (i = 0; i < unitsFE; ++i) {
        a = units[i];

        col = a->collision(t);

        if (col) {
            collisions[collisionsFE++] = col;

            if (col->t < next->t) {
                next = col;
            }
        }

        for (j = i + 1; j < unitsFE; ++j) {
            b = units[j];

            if (a->can(b)) {
                col = a->collision(b, t);

                if (col) {
                    collisions[collisionsFE++] = col;

                    if (col->t < next->t) {
                        next = col;
                    }
                }
            }
        }
    }

    while (t < 1.0) {
        if (next == fake) {
            for (i = 0; i < unitsFE; ++i) {
                units[i]->move(1.0 - t);
            }

            break;
        } else {
            // Move to the collision time
            delta = next->t - t;
            for (i = 0; i < unitsFE; ++i) {
                units[i]->move(delta);
            }

            t = next->t;

            if (next->dir) {
                /*if (doLog) {
                    cerr << next->a->id << " bounce with wall at " << t << endl;
                }*/
                next->a->bounce(next->dir);
            } else {
                /*if (doLog) {
                    cerr << next->a->id << " bounce with " << next->b->id << " at " << t << endl;
                }*/
                next->a->bounce(next->b);
            }

            a = next->a;
            b = next->b;

            // Invalid previous collisions for the concerned units and get new ones
            next = fake;

            for (i = 0; i < collisionsFE; ++i) {
                col = collisions[i];

                if (!mustErase(col, a, b)) {
                    if (col->t < next->t) {
                        next = col;
                    }

                    tempCollisions[tempCollisionsFE++] = col;
                }
            }

            Collision **temp = tempCollisions;
            tempCollisions = collisions;
            collisions = temp;

            collisionsFE = tempCollisionsFE;
            tempCollisionsFE = 0;

            // Find new collisions for a
            col = a->collision(t);
            if (col) {
                collisions[collisionsFE++] = col;

                if (col->t < next->t) {
                    next = col;
                }
            }

            for (i = 0; i < unitsFE; ++i) {
                u = units[i];

                if (a->id != u->id && a->can(u)) {
                    col = a->collision(u, t);

                    if (col) {
                        collisions[collisionsFE++] = col;

                        if (col->t < next->t) {
                            next = col;
                        }
                    }
                }
            }

            // Find new collisions for b
            if (b) {
                col = b->collision(t);

                if (col) {
                    collisions[collisionsFE++] = col;

                    if (col->t < next->t) {
                        next = col;
                    }
                }

                for (i = 0; i < unitsFE; ++i) {
                    u = units[i];

                    if (b->id != u->id && b->can(u)) {
                        col = b->collision(u, t);

                        if (col) {
                            collisions[collisionsFE++] = col;

                            if (col->t < next->t) {
                                next = col;
                            }
                        }
                    }
                }
            }
        }
    }
}

void play() {
    // TEST WITHOUT SPELLS

/*    for (int i = 0; i < 4; ++i) {
        if (spells[i] != NULL)
            spells[i]->apply();
        else
            cerr << "play() method wizard " << i << ": no target defined for spell Windardium!! " <<endl;
    }*/

    bludgers[0]->play();
    bludgers[1]->play();
    wizards[0]->play();
    wizards[1]->play();
    wizards[2]->play();
    wizards[3]->play();


    move();

    for (int i = 0; i < unitsFE; ++i) {
        units[i]->end();
    }

    if (mana != 100) {
        mana += 1;
    }
}

double eval() {
    // Hidden ;)
    int score = 0;
    for (int i = 0; i < snafflesFE; ++i) {
        if (!snaffles[i]->dead) {
            score -= dist(snaffles[i], myGoal);
        }
    }
    return score;
}

void reset() {
    for (int i = 0; i < unitsFE; ++i) {
        units[i]->reset();
    }
    // TEST WITHOUT SPELLS
/*    for (int i = 0; i < 4; ++i) {
        spells[i]->reset();
    }*/

    mana = smana;
    myScore = smyScore;
    hisScore = shisScore;
}

void dummies() {
    if (hisWizard1->snaffle) {
        hisWizard1->snaffle->thrust(500.0, hisGoal, dist(hisWizard1, hisGoal));
    } else {
        Snaffle *target = NULL;
        double targetD = INF;
        double d;

        for (int i = 0; i < snafflesFE; ++i) {
            Snaffle *snaffle = snaffles[i];

            if (!snaffle->dead) {
                d = dist2(hisWizard1, snaffle);

                if (d < targetD) {
                    targetD = d;
                    target = snaffle;
                }
            }
        }

        if (target) {
            hisWizard1->thrust(150.0, target, sqrt(targetD));
        }
    }

    if (hisWizard2->snaffle) {
        hisWizard2->snaffle->thrust(500.0, hisGoal, dist(hisWizard2, hisGoal));
    } else {
        Snaffle *target = NULL;
        double targetD = INF;
        double d;

        for (int i = 0; i < snafflesFE; ++i) {
            Snaffle *snaffle = snaffles[i];

            if (!snaffle->dead) {
                d = dist2(hisWizard2, snaffle);

                if (d < targetD) {
                    targetD = d;
                    target = snaffle;
                }
            }
        }

        if (target) {
            hisWizard2->thrust(150.0, target, sqrt(targetD));
        }
    }
}

void simulate(Solution *solution) {
    energy = 0;
    depth = 0;

//    myWizard1->apply(solution, 0, 1);
//    myWizard2->apply(solution, 0, 2);
//    myWizard1->apply(solution->);
//    myWizard2->apply(solution->moves2);
//    dummies();

#ifdef DEBUG
    cerr << "DEBUG 5 : simulate" <<endl;
#endif
    play();
    depth = 1;

    solution->energy = eval() * 0.1;

    for (int i = 0; i < DEPTH; ++i) {
//        myWizard1->apply(solution, i, 1);
//        myWizard2->apply(solution, i, 2);
        myWizard1->apply(solution->moves1[i]);
        myWizard2->apply(solution->moves2[i]);
        dummies();

        play();
        depth += 1;
    }

    solution->energy += energy + eval();

    reset();
}

void simulate2(Solution *solution) {
    doLog = true;

    cerr << "Solution: " << endl;
    for (int i = 0; i < DEPTH; ++i) {
        cerr << ANGLES[solution->moves1[i]] << " " << ANGLES[solution->moves2[i]] << endl;
    }
    // TEST WITHOUT SPELLS
/*    cerr << "Spell 1: " << solution->spellTurn1 << " " << solution->spell1 << " " << solution->spellTarget1->id << endl;
    cerr << "Spell 2: " << solution->spellTurn2 << " " << solution->spell2 << " " << solution->spellTarget2->id << endl;*/

    energy = 0;
    depth = 0;

//    myWizard1->apply(solution, 0, 1);
//    myWizard2->apply(solution, 0, 2);
    myWizard1->apply(solution->moves1[0]);
    myWizard2->apply(solution->moves2[0]);
    dummies();

    play();

    cerr << "******* State at turn " << depth + 1 << endl;
    for (int i = 0; i < unitsFE; ++i) {
        units[i]->print();
    }
    depth = 1;

    solution->energy = eval() * 0.1;

    for (int i = 0; i < DEPTH; ++i) {
//        myWizard1->apply(solution, i, 1);
//        myWizard2->apply(solution, i, 2);
        myWizard1->apply(solution->moves1[i]);
        myWizard1->apply(solution->moves2[i]);
        dummies();

        play();

        cerr << "******* State at turn " << depth + 1 << endl;
        for (int i = 0; i < unitsFE; ++i) {
            units[i]->print();
        }
        depth += 1;
    }

    solution->energy += energy + eval();

    cerr << "Sanity check : " << solution->energy << endl;
    cerr << "Mana: " << mana << endl;
    cerr << "My score: " << myScore << endl;
    cerr << "His score: " << hisScore << endl;

    reset();

    doLog = false;
}

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

int main() {
    fast_srand(42);

    fake = new Collision();
    fake->t = 1000.0;

    for (int i = 0; i < ANGLES_LENGTH; ++i) {
        cosAngles[i] = cos(ANGLES[i] * TO_RAD);
        sinAngles[i] = sin(ANGLES[i] * TO_RAD);
    }

    cin >> myTeam;
    cin.ignore();
    cerr << myTeam << endl;

    collisionsCache = new Collision *[100];
    collisions = new Collision *[100];
    tempCollisions = new Collision *[100];

    for (int i = 0; i < 100; ++i) {
        collisionsCache[i] = new Collision();
    }

    wizards[0] = new Wizard(0);
    wizards[1] = new Wizard(0);
    wizards[2] = new Wizard(1);
    wizards[3] = new Wizard(1);

    units[0] = wizards[0];
    units[1] = wizards[1];
    units[2] = wizards[2];
    units[3] = wizards[3];

    if (!myTeam) {
        myWizard1 = wizards[0];
        myWizard2 = wizards[1];
        hisWizard1 = wizards[2];
        hisWizard2 = wizards[3];
        myGoal = new Point(16000, 3750);
        hisGoal = new Point(0, 3750);
    } else {
        myWizard1 = wizards[2];
        myWizard2 = wizards[3];
        hisWizard1 = wizards[0];
        hisWizard2 = wizards[1];
        myGoal = new Point(0, 3750);
        hisGoal = new Point(16000, 3750);
    }

    mid = new Point(8000, 3750);

    bludgers[0] = new Bludger();
    bludgers[1] = new Bludger();

    units[4] = bludgers[0];
    units[5] = bludgers[1];

    poles[0] = new Pole(20, 0, 1750);
    poles[1] = new Pole(21, 0, 5750);
    poles[2] = new Pole(22, 16000, 1750);
    poles[3] = new Pole(23, 16000, 5750);

    units[6] = poles[0];
    units[7] = poles[1];
    units[8] = poles[2];
    units[9] = poles[3];

    unitsFE = 10;

    // TEST WITHOUT SPELLS
/*    int spellsFE = 0;
    for (int j = 0; j < 4; ++j)
        spells[spellsFE++] = wizards[j]->spell;*/

    Solution *best = new Solution();

    int oldSnafflesFE;

    while (1) {
        int myScore;
        int myMagic;
        cin >> myScore >> myMagic; cin.ignore();
        cerr << myScore << " " << myMagic <<endl;

        int opponentScore;
        int opponentMagic;
        cin >> opponentScore >> opponentMagic; cin.ignore();
        cerr << opponentScore << " " << opponentMagic <<endl;

        int entities;
        cin >> entities;
        cin.ignore();
        cerr << entities << endl;

        start = NOW;

        int bludgersFE = 0;

        if (turn) {
            for (int i = 0; i < 24; ++i) {
                Unit *u = unitsById[i];

                if (u && u->type == SNAFFLE) {
                    u->dead = true;
                    u->carrier = NULL;
                }
            }
        }

        for (int i = 0; i < entities; ++i) {
            int id; // entity identifier
            string entityType; // "WIZARD", "OPPONENT_WIZARD" or "SNAFFLE" (or "BLUDGER" after first league)
            int x; // position
            int y; // position
            int vx; // velocity
            int vy; // velocity
            int state; // 1 if the wizard is holding a Snaffle, 0 otherwise
            cin >> id >> entityType >> x >> y >> vx >> vy >> state;
            cin.ignore();
            cerr << id << " " << entityType << " " << x << " " << y << " " << vx << " " << vy << " " << state <<endl;

            Unit *unit;

            if (entityType == "WIZARD" || entityType == "OPPONENT_WIZARD") {
                unit = wizards[id];
            } else if (entityType == "SNAFFLE") {
                if (!turn) {
                    unit = new Snaffle();
                } else {
                    unit = unitsById[id];
                }

                unit->dead = false;
                units[unitsFE++] = unit;
                snaffles[snafflesFE++] = (Snaffle *) unit;
            } else if (entityType == "BLUDGER") {
                unit = bludgers[bludgersFE++];
            }

            unit->update(id, x, y, vx, vy, state);
            // unit->update(id, y, x, vy, vx, state);
        }

        if (turn == 0) {
            victory = (snafflesFE / 2) + 1;

            for (int i = 0; i < unitsFE; ++i) {
                unitsById[units[i]->id] = units[i];
            }
        }

        // Mise à jour des carriers et des snaffles
        for (int i = 0; i < 4; ++i) {
            wizards[i]->updateSnaffle();
        }

        // Mise à jour du score
        if (turn && oldSnafflesFE != snafflesFE) {
            for (int i = 0; i < 24; ++i) {
                Unit *u = unitsById[i];

                if (u && u->type == SNAFFLE && u->dead) {
                    if (!myTeam) {
                        if (u->x > 8000) {
                            myScore += 1;
                        } else {
                            hisScore += 1;
                        }
                    } else {
                        if (u->x > 8000) {
                            hisScore += 1;
                        } else {
                            myScore += 1;
                        }
                    }

                    delete u;
                    unitsById[i] = NULL;
                }
            }
        }

        // TEST WITHOUT SPELLS
/*
        // Cibles pour les sorts

        // Bludgers pour tous les sorts
        spellTargets[0] = bludgers[0];
        spellTargets[1] = bludgers[1];
        spellTargetsFE = 2;

        // Wizards ennemis pour wingardium
        if (!myTeam) {
            spellTargets[spellTargetsFE++] = wizards[2];
            spellTargets[spellTargetsFE++] = wizards[3];
        } else {
            spellTargets[spellTargetsFE++] = wizards[0];
            spellTargets[spellTargetsFE++] = wizards[1];
        }

        // Snaffles pour tous les sorts sauf obliviate
        for (int j = 0; j < snafflesFE; ++j)
            spellTargets[spellTargetsFE++] = snaffles[j];
*/
        for (int i = 0; i < unitsFE; i++) {
            units[i]->save();
            smana = mana;
            smyScore = myScore;
            shisScore = hisScore;
        }


/*        for (int i = 0; i < 4; i++) {
            if (spells[i]->target != NULL) {
                spells[i]->print();
                spells[i]->reloadTarget();
                spells[i]->save();
            }
            else
                cerr << "Wizard #" << i << ": no target defined for spell Windardium!! " <<endl;
        }*/


#ifndef PROD
        if (turn) {
            for (int i = 0; i < unitsFE; i++) {
                units[i]->compare();
            }
        }
        cerr << "DEBUG 1 " <<endl;
#endif

#ifndef PROD
        cerr << "hisScore : " << hisScore << endl;
        cerr << "victory : " << victory << endl;
#endif

        /*cerr << "***** State for this turn " << endl;
        cerr << "Mana: " << mana << endl;
        cerr << "My score: " << myScore << endl;
        cerr << "His score: " << hisScore << endl;
        for (int i = 0; i < unitsFE; i++) {
            units[i]->print();
        }*/

        // Evolution
        cerr << "DEBUG 2" << endl;

        Solution *base;
        if (turn) {
            cerr << "DEBUG 3" << best << endl;
            base = new Solution();

            for (int j = 1; j < DEPTH; ++j) {
                base->moves1[j - 1] = best->moves1[j];
                base->moves2[j - 1] = best->moves2[j];
            }
            cerr << "DEBUG 2" << best << endl;

            // TEST WITHOUT SPELLS
/*
            base->spellTurn1 = best->spellTurn1;
            base->spellTarget1 = best->spellTarget1;
            base->spellTurn2 = best->spellTurn2;
            base->spellTarget2 = best->spellTarget2;

            if (!base->spellTurn1) {
                base->spellTurn1 = SPELL_DEPTH - 1;
            } else {
                base->spellTurn1 -= 1;
            }

            if (!base->spellTurn2) {
                base->spellTurn2 = SPELL_DEPTH - 1;
            } else {
                base->spellTurn2 -= 1;
            }

            if (base->spellTarget1->dead) {
                base->spellTurn1 = SPELL_DEPTH - 1;
                base->spellTarget1 = spellTargets[fastRandInt(spellTargetsFE)];
            }

            if (base->spellTarget2->dead) {
                base->spellTurn2 = SPELL_DEPTH - 1;
                base->spellTarget2 = spellTargets[fastRandInt(spellTargetsFE)];
            }*/
            cerr << "Solution" << best << endl;

            delete best;
        }


        cerr << "DEBUG 4" << endl;

        Solution **pool = new Solution *[POOL];
        Solution **newPool = new Solution *[POOL];
        Solution **temp;
        int counter = POOL;


        best = new Solution();
        Solution *sol = new Solution();
        sol->randomize();

        cerr << "DEBUG 5" << endl;

        simulate(sol);
        pool[0] = sol;


        best->copy(sol);

        Solution *tempBest = sol;

        cerr << "DEBUG 6" << endl;

        // First generation
        int startI = 1;

        if (turn) {
            cerr << "DEBUG 7" << endl;
            // Populate the POOL with some copy of the previous best one
            for (int i = startI; i < POOL / 5; ++i) {
                Solution *solution = new Solution();
                solution->copy(base);

                // Add a last one random
                solution->moves1[DEPTH - 1] = fastRandInt(ANGLES_LENGTH);
                solution->moves2[DEPTH - 1] = fastRandInt(ANGLES_LENGTH);

                simulate(solution);

                if (solution->energy > tempBest->energy) {
                    tempBest = solution;
                }

                pool[i] = solution;
            }

            delete base;

            startI = POOL / 5;
        }

        cerr << "DEBUG 8" << endl;

        for (int i = startI; i < POOL; i++) {
            Solution *solution = new Solution();
            solution->randomize();

            simulate(solution);

            if (solution->energy > tempBest->energy) {
                tempBest = solution;
            }

            pool[i] = solution;
        }

        cerr << "DEBUG 9" << endl;

        if (tempBest->energy > best->energy) {
            best->copy(tempBest);
        }
        tempBest = best;

        double limit = turn ? 0.085 : 0.800;

        cerr << "DEBUG 10" << endl;

        for (int i=0; i < DEPTH; i++)
            cerr << "best moves: (" << best->moves1[i] << ", " << best->moves2[i] << ")" <<endl;

#ifdef DEBUG
#define LIMIT counter <= 1000
#else
#define LIMIT TIME < limit
#endif

        int generation = 1;
        int bestGeneration = 1;

        int poolFE;
        while (LIMIT) {
            // New generation

            // Force the actual best with a mutation to be in the pool
            Solution *solution = new Solution();
            solution->copy(tempBest);
            solution->mutate();
            simulate(solution);

            // cerr << "DEBUG 11" << endl;

            if (solution->energy > tempBest->energy) {
                tempBest = solution;
            }

            newPool[0] = solution;

            counter += 1;

            poolFE = 1;
            while (poolFE < POOL && LIMIT) {
                int aIndex = fastRandInt(POOL);
                int bIndex;

                do {
                    bIndex = fastRandInt(POOL);
                } while (bIndex == aIndex);

                int firstIndex = pool[aIndex]->energy > pool[bIndex]->energy ? aIndex : bIndex;

                do {
                    aIndex = fastRandInt(POOL);
                } while (aIndex == firstIndex);

                do {
                    bIndex = fastRandInt(POOL);
                } while (bIndex == aIndex || bIndex == firstIndex);

                int secondIndex = pool[aIndex]->energy > pool[bIndex]->energy ? aIndex : bIndex;

                Solution *child = pool[firstIndex]->merge(pool[secondIndex]);

                if (!fastRandInt(MUTATION)) {
                    child->mutate();
                }

                simulate(child);

                if (child->energy > tempBest->energy) {
                    tempBest = child;
                }

                newPool[poolFE++] = child;

                counter += 1;
            }

            // Burn previous generation !!
            for (int i = 0; i < POOL; i++) {
                delete pool[i];
            }

            temp = pool;
            pool = newPool;
            newPool = temp;

            if (tempBest->energy > best->energy) {
                best->copy(tempBest);
                bestGeneration = generation;
            }
            tempBest = best;

            generation += 1;
        }

#ifndef PROD
        cerr << "Counter: " << counter << endl;
        cerr << "Energy: " << best->energy << endl;
        cerr << "Generation: " << generation << endl;
#endif


#ifdef DEBUG
        // Play a last time for debug
        simulate(best);
#endif

        cerr << "DEBUG 12" << endl;

        cerr << "best = " << best << endl;

        // Play a last time to check some infos
//        myWizard1->apply(best, 0, 1);
//        myWizard2->apply(best, 0, 2);
        myWizard1->apply(best->moves1[0]);
        myWizard2->apply(best->moves2[0]);
        dummies();

        play();

        cerr << "***** State for next turn " << endl;
        cerr << "Mana: " << mana << endl;
        cerr << "My score: " << myScore << endl;
        cerr << "His score: " << hisScore << endl;
        for (int i = 0; i < unitsFE; i++) {
            units[i]->print();
        }


        smana = mana;
        bludgers[0]->slast = bludgers[0]->last;
        bludgers[1]->slast = bludgers[1]->last;

        // TEST WITHOUT SPELLS
/*
        for (int i = 0; i < 4; i++) {
            spells[i]->save();
        }
*/

#ifndef PROD
        for (int i = 0; i < unitsFE; i++) {
            units[i]->store();
        }
#endif

        reset();

#ifdef PROFILE
        double totalSpent = 0;
            for (int i = 0; i < DURATIONS_COUNT; i++) {
                totalSpent += durations[i];
            }
            for (int i = 0; i < DURATIONS_COUNT; i++) {
                fprintf(stderr, "Time %3d: %.6fms (%.2f%%)\n", i, durations[i] * 1000.0, durations[i] * 100.0/totalSpent);
            }
            fprintf(stderr, "Total: %.6fms (%.6fms per turn)\n", totalSpent * 1000.0, totalSpent * 1000.0/(double)(turn + 1));
#endif

        myWizard1->output(best->moves1[0], best->spellTurn1, best->spellTarget1);
        myWizard2->output(best->moves2[0], best->spellTurn2, best->spellTarget2);

        // Burn last generation !!
        for (int i = 0; i < poolFE; i++) {
            delete pool[i];
        }

        delete[] pool;
        delete[] newPool;

        turn += 1;
        unitsFE = 10;

        oldSnafflesFE = snafflesFE;
        snafflesFE = 0;
    }
}