#include "Game.h"
#include "Engine.h"
#include "data/List.h"
#include "input/Controller.h"

struct Line final {
    Vector2 a;
    Vector2 b;
    Color4 color;
    float friction;

    Line(const Vector2& a, const Vector2& b, Color4 color, float friction)
        : a(a), b(b), color(color), friction(friction) {
    }

    Line(const Vector2& a, const Vector2& b)
        : a(a), b(b), color(Color4(0xFF, 0xFF, 0xFF, 0xFF)), friction(0.0f) {
    }
};

static List<Line> lines;

static constexpr float SPEED = 2.5f;
static constexpr float GRAVITY = 0.5f;
static constexpr float COLLISION_STEP = 0.005f;

static Vector2 windowSize;
static Vector2 lastPosition;
static Vector2 position{150.0f, 50.0f};
static Vector2 size{50.0f, 50.0f};
static Vector2 velocity;
static Vector2 acceleration;
static Vector2 drag;
static float mass = 5.0f;
static bool onGround = false;
static float jumpPower = 0.0f;
static float steepness = 0.0f;
static float friction = 0.0f;

static bool physicsToggle = true;

static Vector2 waterPosition;
static Vector2 waterSize;

static Vector2 projectileLastPosition;
static Vector2 projectilePosition;
static Vector2 projectileVelocity;
static Vector2 projectileSize{20.0f, 20.0f};

static float lastAngle;
static float angle;
static float angleVelocity = 5.0f;

static void addForce(const Vector2& force) {
    acceleration += force / mass;
}

static void addMovement() {
    physicsToggle =
        physicsToggle ^ Window::Controls::wasReleased(Controller::start);
    float movement = Window::Controls::isDown(Controller::right) -
                     Window::Controls::isDown(Controller::left);
    float actualSpeed = SPEED * (1.0f - std::abs(steepness) * onGround);
    if(physicsToggle) {
        addForce(Vector2(actualSpeed, 0.0f) * movement);
    } else {
        // movement by velocity should have the same terminal velocity as
        // movement by force
        float terminalFactor = 1.0f / ((1.0 - drag[0]) * drag[0]);
        velocity[0] = (actualSpeed / mass) * terminalFactor * movement;
    }
}

static bool isIn(const Vector2& posA, const Vector2& sizeA, const Vector2& posB,
                 const Vector2& sizeB) {
    Vector2 maxA = posA + sizeA;
    Vector2 maxB = posB + sizeB;
    return posA[0] < maxB[0] && maxA[0] > posB[0] && posA[1] < maxB[1] &&
           maxA[1] > posB[1];
}

float ERROR = 1.0f / 512.0f;

static bool isBetween(float y, float y1, float y2) {
    return y >= std::min(y1, y2) && y <= std::max(y1, y2);
}

static bool compareFloats(float a, float b) {
    return std::abs(a - b) < ERROR;
}

static bool intersect(float x11, float y11, float x12, float y12, float x21,
                      float y21, float x22, float y22) {
    if(compareFloats(x11, x12)) {
        if(compareFloats(x21, x22)) {
            return false;
        } else {
            if(!isBetween(x11, x21, x22)) {
                return false;
            }
            float k = (y21 - y22) / (x21 - x22);
            float d = y22 - k * x22;
            float y = d + x11 * k;
            return isBetween(y, y11, y12) && isBetween(y, y21, y22);
        }
    } else {
        if(compareFloats(x21, x22)) {
            if(!isBetween(x21, x11, x12)) {
                return false;
            }
            float k = (y11 - y12) / (x11 - x12);
            float d = y12 - k * x12;
            float y = d + x21 * k;
            return isBetween(y, y11, y12) && isBetween(y, y21, y22);
        } else {
            float k1 = (y11 - y12) / (x11 - x12);
            float k2 = (y21 - y22) / (x21 - x22);
            if(compareFloats(k1, k2)) {
                return false;
            }
            float d1 = y12 - k1 * x12;
            float d2 = y22 - k2 * x22;

            float x = (d1 - d2) / (k2 - k1);
            if(!isBetween(x, x11, x12) || !isBetween(x, x21, x22)) {
                return false;
            }
            float y = k1 * x + d1;
            return isBetween(y, y11, y12) && isBetween(y, y21, y22);
        }
    }
}

static bool areColliding(const Line& a, const Line& b) {
    return intersect(a.a[0], a.a[1], a.b[0], a.b[1], b.a[0], b.a[1], b.b[0],
                     b.b[1]);
}

static bool doesPlayerCollide() {
    Vector2 posX = position + Vector2(size[0], 0.0f);
    Vector2 posY = position + Vector2(0.0f, size[1]);
    Vector2 posXY = position + size;
    for(const Line& line : lines) {
        if(areColliding(line, {position, posX}) ||
           areColliding(line, {position, posY}) ||
           areColliding(line, {posX, posXY}) ||
           areColliding(line, {posY, posXY})) {
            return true;
        }
    }
    return false;
}

static void slopeSteepnessAndFriction() {
    Line left{position - Vector2(0.0f, 10.0f * COLLISION_STEP),
              position + Vector2(0.0f, size[1])};
    Line right{position + Vector2(size[0], -10.0f * COLLISION_STEP),
               position + size};
    float min = 100.0f;
    float value = 100.0f;
    for(const Line& line : lines) {
        if(areColliding(line, left) || areColliding(line, right)) {
            float f = (line.a[1] - line.b[1]) / (line.a[0] - line.b[0]);
            if(std::abs(f) < min) {
                min = std::abs(f);
                value = f;
                friction = line.friction;
            }
        }
    }
    steepness = value;
}

static void applyDrag() {
    waterPosition = windowSize * Vector2(0.5f, 0.0f);
    waterSize = windowSize * Vector2(0.5f, 0.125f);
    drag = Vector2(0.1f, 0.1f);
    if(isIn(position, size, waterPosition, waterSize)) {
        drag *= 4.0f;
    }
    acceleration -= velocity * drag;
}

static void applyGravity() {
    acceleration -= Vector2(0.0f, GRAVITY);
}

static void handleJump() {
    bool justJumped = false;
    if(Window::Controls::isDown(Controller::a) && onGround) {
        jumpPower = 15.0f;
        onGround = false;
        justJumped = true;
    }
    addForce(Vector2(0.0f, jumpPower));
    jumpPower *= 0.9f;
    jumpPower *= Window::Controls::isDown(Controller::a) &&
                 (velocity[1] > 0.0f || justJumped);
}

static void relaunchProjectile() {
    if(projectilePosition[1] >= 0.0f &&
       !isIn(position, size, projectilePosition, projectileSize)) {
        return;
    }
    projectilePosition = windowSize * Vector2(0.0f, 0.5f);
    projectileLastPosition = projectilePosition;

    Vector2 start = projectilePosition;
    Vector2 end = position + size * 0.5f;
    constexpr float strengh = 25.0f;

    float best = 100000000.0f;
    float bestRad = 0.0f;

    for(float angle = -30.0f; angle < 90.0f; angle += 0.25f) {
        float rad = angle * 0.017453293f;
        Vector2 v(strengh * cosf(rad), strengh * sinf(rad));

        float t = (end[0] - start[0]) / v[0];
        float y = v[1] * t - GRAVITY * t * t * 0.5f + start[1];

        float diff = std::abs(y - end[1]);
        if(diff < best) {
            bestRad = rad;
            best = diff;
        }
    }

    projectileVelocity =
        Vector2(strengh * cosf(bestRad), strengh * sinf(bestRad));
}

static float sign(float f) {
    return (f > 0.0f) - (f < 0.0f);
}

static void applyFriction() {
    if(!onGround) {
        return;
    }
    float rad = atanf(steepness);
    float c = cosf(rad);
    float fnLength = mass * GRAVITY * c;
    Vector2 fn(fnLength * c, fnLength * sinf(rad)); // already swapt
    Vector2 fullFriction = fn * friction;
    fullFriction =
        Vector2(std::abs(fullFriction[0]), std::abs(fullFriction[1]));

    float signX = sign(acceleration[0]);
    float signY = sign(acceleration[1]);

    fullFriction *= -Vector2(signX, signY);

    addForce(fullFriction);
    if(sign(acceleration[0]) != signX) {
        acceleration[0] = 0.0f;
    }
    if(sign(acceleration[1]) != signY) {
        acceleration[1] = 0.0f;
    }
}

static void addLines() {
    lines.clear();
    lines.add(Line({0.0f, 0.0f}, windowSize * Vector2(1.0f, 0.0f)));
    lines.add(Line({0.0f, 0.0f}, windowSize * Vector2(0.0f, 1.0f)));
    lines.add(Line(windowSize, windowSize * Vector2(1.0f, 0.0f)));
    lines.add(windowSize * Vector2(0.75f, 0.0f),
              windowSize * Vector2(1.0f, 0.5f));
    lines.add(Line({0.0f, 30.0f}, {400.0f, 0.0f}));
    lines.add(Line({0.0f, 60.0f}, {300.0f, 0.0f}));
    lines.add(Line({0.0f, 90.0f}, {200.0f, 0.0f}));
    lines.add(Line({0.0f, 120.0f}, {100.0f, 0.0f}));
    lines.add(Line({100.0f, 180.0f}, {200.0f, 180.0f}));

    lines.add(Line({200.0f, 180.0f}, {300.0f, 230.0f},
                   Color4(0xF0, 0x00, 0x00, 0xFF), 0.2f));
    lines.add(Line({300.0f, 230.0f}, {400.0f, 280.0f},
                   Color4(0xC0, 0x00, 0x00, 0xFF), 0.4f));
    lines.add(Line({400.0f, 280.0f}, {500.0f, 330.0f},
                   Color4(0x90, 0x00, 0x00, 0xFF), 0.6f));
    lines.add(Line({500.0f, 330.0f}, {600.0f, 380.0f},
                   Color4(0x60, 0x00, 0x00, 0xFF), 0.8f));

    lines.add(Line({300.0f, 140.0f}, {400.0f, 200.0f},
                   Color4(0xF0, 0x00, 0x00, 0xFF), 0.2f));
    lines.add(Line({400.0f, 200.0f}, {500.0f, 220.0f},
                   Color4(0xC0, 0x00, 0x00, 0xFF), 0.4f));
    lines.add(Line({500.0f, 220.0f}, {600.0f, 240.0f},
                   Color4(0x90, 0x00, 0x00, 0xFF), 0.6f));
    lines.add(Line({600.0f, 240.0f}, {700.0f, 260.0f},
                   Color4(0x60, 0x00, 0x00, 0xFF), 0.8f));

    lines.add(Line({600.0f, 380.0f}, {1000.0f, 580.0f}));
}

void Game::tick() {
    windowSize = Engine::getSize().toFloat();
    lastPosition = position;
    acceleration = Vector2();

    applyGravity();
    addMovement();
    handleJump();
    applyFriction();
    applyDrag();

    if(steepness != 100.0f) {
        if(steepness < -1.0f) {
            acceleration[0] += 0.1f;
        } else if(steepness > 1.0f) {
            acceleration[0] -= 0.1f;
        }
    }

    addLines();

    velocity += acceleration;

    onGround = false;
    Vector2 energy = velocity;
    while(energy[0] != 0.0f || energy[1] != 0.0f) {
        for(int i = 0; i < 2; i++) {
            if(energy[i] != 0.0f) {
                float old = position[i];
                if(energy[i] > COLLISION_STEP) {
                    position[i] += COLLISION_STEP;
                    energy[i] -= COLLISION_STEP;
                } else if(energy[i] < -COLLISION_STEP) {
                    position[i] -= COLLISION_STEP;
                    energy[i] += COLLISION_STEP;
                } else {
                    position[i] += energy[i];
                    energy[i] = 0.0f;
                }
                if(doesPlayerCollide()) {
                    if(i == 0) {
                        float oldY = position[1];
                        position[1] += COLLISION_STEP;
                        if(!doesPlayerCollide()) {
                            continue;
                        }
                        position[1] = oldY;
                    }
                    energy[i] = 0.0f;
                    velocity[i] = 0.0f;
                    position[i] = old;
                }
            }
        }
    }
    float oldSteepness = steepness;
    slopeSteepnessAndFriction();
    onGround = std::abs(steepness) < 1.0f;
    if(onGround) {
        velocity[1] = 0.0f;
    }
    if(steepness == 100.0f) {
        steepness = oldSteepness;
    }

    projectileLastPosition = projectilePosition;
    projectileVelocity -= Vector2(0.0f, GRAVITY);
    projectilePosition += projectileVelocity;
    relaunchProjectile();

    lastAngle = angle;
    angle += angleVelocity;
}

void Game::render(float lag, Renderer& r) {
    r.translateTo(0.0f, 0.0f)
        .scale(1.0f, -1.0f)
        .translateY(Engine::getSize()[1])
        .update();
    r.drawRectangle(waterPosition, waterSize, Color4(0x00, 0x00, 0xFF, 0xFF));

    Vector2 pos = Math::interpolate(lastPosition, position, lag);

    r.push();
    r.translateTo(0.0f, 0.0f)
        .translate(-size * 0.5f)
        .rotate(atanf(steepness) * 180.0f / M_PI)
        .translate(pos)
        .translate(size * 0.5f)
        .scale(1.0f, -1.0f)
        .translateY(Engine::getSize()[1])
        .update();
    r.drawRectangle(Vector2(), size, Color4(0xFF, 0x00, 0x00, 0xFF));
    r.pop();
    r.update();

    for(int i = 0; i < 360; i += 120) {
        r.push();
        r.translateTo(0.0f, 0.0f)
            .translate(size * 0.75f)
            .rotate(Math::interpolate(lastAngle, angle, lag) + i)
            .translate(pos + size * 0.5f)
            .scale(1.0f, -1.0f)
            .translateY(Engine::getSize()[1])
            .update();
        r.drawRectangle(Vector2(), size * 0.25f,
                        Color4(0xFF, 0x00, 0xFF, 0xFF));
        r.pop();
        r.update();
    }

    r.drawRectangle(
        Math::interpolate(projectileLastPosition, projectilePosition, lag),
        projectileSize, Color4(0xFF, 0xFF, 0x00, 0xFF));

    for(const Line& line : lines) {
        Vector2 dir = line.b - line.a;
        Vector2 normal(dir[1], -dir[0]);
        normal.normalize();
        normal *= 2.0f;
        Vector2 ap = line.a + normal;
        Vector2 bp = line.b + normal;
        Vector2 an = line.a - normal;
        Vector2 bn = line.b - normal;
        r.drawTriangle(ap, bp, an, line.color);
        r.drawTriangle(bp, an, bn, line.color);
    }

    r.translateTo(0.0f, 0.0f).update();
    r.setStringSize(2);

    float y = 10.0f;
    y = r.drawString(10.0f, y,
                     physicsToggle ? "Force Physics" : "Velocity Physics");
    StringBuffer<100> s("a = ");
    s.append(acceleration).append(" before collision");
    y = r.drawString(10.0f, y, s);
    s.clear().append("v = ").append(velocity).append(" after collision");
    y = r.drawString(10.0f, y, s);
    s.clear().append("Ground = ").append(onGround);
    y = r.drawString(10.0f, y, s);
    s.clear().append("Angle = ").append(atanf(steepness) * 57.295779513f);
    y = r.drawString(10.0f, y, s);
    s.clear().append("Friction = ").append(friction);
    y = r.drawString(10.0f, y, s);
}

bool Game::isRunning() {
    return !Window::Controls::isDown(Controller::select);
}