#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <stdbool.h>
#include <unistd.h>
#include <termios.h>
#include <limits.h>
#include <time.h>
#include <sys/time.h>
#include <SDL2/SDL.h>
#include <SDL2/SDL_image.h>

#include "structure.h"
#include "base.h"
#include "cars.h"
#include "music.h"
#include "collisions.h"

// use after updating the position
void updateChunk(level* lvl, int nPl) {
    car* c = players[nPl].c;
    if(c->pos.fx < 0.0) {
        c->pos.fx += 1.0*ROOM_SIZE;
        if(c->chx > 0){c->chx -= 1;}
    }
    if(c->pos.fx >= 1.0*ROOM_SIZE) {
        c->pos.fx -= 1.0*ROOM_SIZE;
        if(c->chx < lvl->cols){c->chx += 1;}
    }
    if(c->pos.fy < 0.0) {
        c->pos.fy += 1.0*ROOM_SIZE;
        if(c->chy > 0){c->chy -= 1;}
    }
    if(c->pos.fy >= 1.0*ROOM_SIZE) {
        c->pos.fy -= 1.0*ROOM_SIZE;
        if(c->chy < lvl->lines){c->chy += 1;}
    }
}

ptf get_absolute_coords(int nPl) {
    return (ptf){
        .fx = players[nPl].c->pos.fx + ROOM_SIZE*players[nPl].c->chx,
        .fy = players[nPl].c->pos.fy + ROOM_SIZE*players[nPl].c->chy
    };
}

// -------------------------------------------------------------------------------- //
// make the player move DT (does not take friction into consideration)

// start
void move_on_START(int nPl) {
    car* c = players[nPl].c;
    c->pos.fx += c->vel.fx*DT;
    c->pos.fy += c->vel.fy*DT;
    if(
        (START_DIR != 3 && c->pos.fx-PLAYER_R <= 0.0 && c->vel.fx < 0.0) ||
        (START_DIR == 3 && c->pos.fx-PLAYER_R <= 0.0 && c->vel.fx < 0.0 && (c->pos.fy-PLAYER_R <= ROOM_SIZE*DIST_EDGE || c->pos.fy+PLAYER_R >= ROOM_SIZE*(1.0-DIST_EDGE)))
    ) {         // left wall
        c->vel.fx *= -1.0;
        c->pos.fx += 2.0*c->vel.fx*DT;
        c->vel.fx *= RESTITUTION_WALL;
    }
    if(
        (START_DIR != 1 && c->pos.fx+PLAYER_R >= 1.0*ROOM_SIZE && c->vel.fx > 0.0) ||
        (START_DIR == 1 && c->pos.fx+PLAYER_R >= 1.0*ROOM_SIZE && c->vel.fx > 0.0 && (c->pos.fy-PLAYER_R <= ROOM_SIZE*DIST_EDGE || c->pos.fy+PLAYER_R >= ROOM_SIZE*(1.0-DIST_EDGE)))
    ) {         // right wall
        c->vel.fx *= -1.0;
        c->pos.fx += 2.0*c->vel.fx*DT;
        c->vel.fx *= RESTITUTION_WALL;
    }
    if(
        (START_DIR != 0 && c->pos.fy-PLAYER_R <= 0.0 && c->vel.fy < 0.0) ||
        (START_DIR == 0 && c->pos.fy-PLAYER_R <= 0.0 && c->vel.fy < 0.0 && (c->pos.fx-PLAYER_R <= ROOM_SIZE*DIST_EDGE || c->pos.fx+PLAYER_R >= ROOM_SIZE*(1.0-DIST_EDGE)))
    ) {         // top wall
        c->vel.fy *= -1.0;
        c->pos.fy += 2.0*c->vel.fy*DT;
        c->vel.fy *= RESTITUTION_WALL;
    }
    if(
        (START_DIR != 2 && c->pos.fy+PLAYER_R >= 1.0*ROOM_SIZE && c->vel.fy > 0.0) ||
        (START_DIR == 2 && c->pos.fy+PLAYER_R >= 1.0*ROOM_SIZE && c->vel.fy > 0.0 && (c->pos.fx-PLAYER_R <= ROOM_SIZE*DIST_EDGE || c->pos.fx+PLAYER_R >= ROOM_SIZE*(1.0-DIST_EDGE)))
    ) {         // bottom wall
        c->vel.fy *= -1.0;
        c->pos.fy += 2.0*c->vel.fy*DT;
        c->vel.fy *= RESTITUTION_WALL;
    }
}

// finish
void move_on_FINISH(int nPl) {
    car* c = players[nPl].c;
    c->pos.fx += c->vel.fx*DT;
    c->pos.fy += c->vel.fy*DT;
    if(
        (c->pos.fx-PLAYER_R <= 0.0 && c->vel.fx < 0.0)
    ) {         // left wall
        c->vel.fx *= -1.0;
        c->pos.fx += 2.0*c->vel.fx*DT;
        c->vel.fx *= RESTITUTION_WALL;
    }
    if(
        (c->pos.fx+PLAYER_R >= 1.0*ROOM_SIZE && c->vel.fx > 0.0)
    ) {         // right wall
        c->vel.fx *= -1.0;
        c->pos.fx += 2.0*c->vel.fx*DT;
        c->vel.fx *= RESTITUTION_WALL;
    }
    if(
        (c->pos.fy-PLAYER_R <= 0.0 && c->vel.fy < 0.0)
    ) {         // top wall
        c->vel.fy *= -1.0;
        c->pos.fy += 2.0*c->vel.fy*DT;
        c->vel.fy *= RESTITUTION_WALL;
    }
    if(
        (c->pos.fy+PLAYER_R >= 1.0*ROOM_SIZE && c->vel.fy > 0.0)
    ) {         // bottom wall
        c->vel.fy *= -1.0;
        c->pos.fy += 2.0*c->vel.fy*DT;
        c->vel.fy *= RESTITUTION_WALL;
    }
}

void move_on_STR_V(int nPl) {
    car* c = players[nPl].c;
    c->pos.fx += c->vel.fx*DT;
    c->pos.fy += c->vel.fy*DT;
    if(c->pos.fx-PLAYER_R <= ROOM_SIZE*DIST_EDGE && c->vel.fx < 0.0) {         // left wall
        c->vel.fx *= -1.0;
        c->pos.fx += 2.0*c->vel.fx*DT;
        c->vel.fx *= RESTITUTION_WALL;
    }
    if(c->pos.fx+PLAYER_R >= ROOM_SIZE*(1.0-DIST_EDGE) && c->vel.fx > 0.0) {     // right wall
        c->vel.fx *= -1.0;
        c->pos.fx += 2.0*c->vel.fx*DT;
        c->vel.fx *= RESTITUTION_WALL;
    }
}

void move_on_STR_H(int nPl) {
    car* c = players[nPl].c;
    c->pos.fx += c->vel.fx*DT;
    c->pos.fy += c->vel.fy*DT;
    if(c->pos.fy-PLAYER_R <= ROOM_SIZE*DIST_EDGE && c->vel.fy < 0.0) {         // top wall
        c->vel.fy *= -1.0;
        c->pos.fy += 2.0*c->vel.fy*DT;
        c->vel.fy *= RESTITUTION_WALL;
    }
    if(c->pos.fy+PLAYER_R >= ROOM_SIZE*(1.0-DIST_EDGE) && c->vel.fy > 0.0) {     // bottom wall
        c->vel.fy *= -1.0;
        c->pos.fy += 2.0*c->vel.fy*DT;
        c->vel.fy *= RESTITUTION_WALL;
    }
}

void move_on_TURN(int nPl, int cenX, int cenY) {
    car* c = players[nPl].c;
    ptf prev = (ptf){.fx = c->pos.fx, .fy = c->pos.fy};
    c->pos.fx += c->vel.fx*DT;
    c->pos.fy += c->vel.fy*DT;
    ptf next = c->pos;

    double prevDist = distance(prev, (ptf){.fx = 1.0*cenX, .fy = 1.0*cenY});
    double nextDist = distance(next, (ptf){.fx = 1.0*cenX, .fy = 1.0*cenY});

    // inner circle
    if(prevDist-PLAYER_R > ROOM_SIZE*DIST_EDGE && nextDist-PLAYER_R <= ROOM_SIZE*DIST_EDGE) {
        ptf to_in = normalize((ptf){
            .fx = cenX-prev.fx,
            .fy = cenY-prev.fy
        });
        double inwards = dot(to_in, c->vel);
        c->vel = add(c->vel, (ptf){.fx = -2.0*to_in.fx*inwards, .fy = -2.0*to_in.fy*inwards});

        c->pos.fx += 2.0*c->vel.fx*DT;
        c->pos.fy += 2.0*c->vel.fy*DT;
    }

    // in case someone clips (inner)
    if(nextDist-prevDist < 0.0 && prevDist-PLAYER_R < ROOM_SIZE*DIST_EDGE) {
        ptf to_in = normalize((ptf){
            .fx = cenX-prev.fx,
            .fy = cenY-prev.fy
        });
        double inwards = dot(to_in, c->vel);
        c->vel = add(c->vel, (ptf){.fx = -2.0*to_in.fx*inwards, .fy = -2.0*to_in.fy*inwards});

        c->pos.fx += 2.0*c->vel.fx*DT;
        c->pos.fy += 2.0*c->vel.fy*DT;
    }

    // outer circle
    if(prevDist+PLAYER_R < ROOM_SIZE*(1.0-DIST_EDGE) && nextDist+PLAYER_R >= ROOM_SIZE*(1.0-DIST_EDGE)) {
        ptf to_out = normalize((ptf){
            .fx = prev.fx-cenX,
            .fy = prev.fy-cenY
        });
        double outwards = dot(to_out, c->vel);
        c->vel = add(c->vel, (ptf){.fx = -2.0*to_out.fx*outwards, .fy = -2.0*to_out.fy*outwards});

        c->pos.fx += 2.0*c->vel.fx*DT;
        c->pos.fy += 2.0*c->vel.fy*DT;
    }

    // in case someone clips (outer)
    if(nextDist-prevDist > 0.0 && prevDist-PLAYER_R > ROOM_SIZE*(1.0-DIST_EDGE)) {
        ptf to_in = normalize((ptf){
            .fx = cenX-prev.fx,
            .fy = cenY-prev.fy
        });
        double inwards = dot(to_in, c->vel);
        c->vel = add(c->vel, (ptf){.fx = -2.0*to_in.fx*inwards, .fy = -2.0*to_in.fy*inwards});

        c->pos.fx += 2.0*c->vel.fx*DT;
        c->pos.fy += 2.0*c->vel.fy*DT;
    }
}

void bumpOtherCars(int nPl) {
    for(int p = 0; p < nPlayers; p++) {
        if(p != nPl && 0==winners[p]) {
            double sep = distance(
                get_absolute_coords(p),
                get_absolute_coords(nPl)
            );
            if(sep <= 2*PLAYER_R) {
                // p_total is multiplied by RESTITUTION_PLAYER
                ptf director = normalize((ptf){
                    .fx = players[p].c->pos.fx - players[nPl].c->pos.fx,
                    .fy = players[p].c->pos.fy - players[nPl].c->pos.fy
                });
                double speed_P = norm(players[p].c->vel);
                double speed_N = norm(players[nPl].c->vel);

                players[p].c->vel.fx = director.fx*RESTITUTION_PLAYER*(speed_N);
                players[p].c->vel.fy = director.fy*RESTITUTION_PLAYER*(speed_N);

                players[nPl].c->vel.fx = -director.fx*RESTITUTION_PLAYER*(speed_P);
                players[nPl].c->vel.fy = -director.fy*RESTITUTION_PLAYER*(speed_P);

                // snap players away from each other (so they dont overlap)
                while(distance(get_absolute_coords(p),get_absolute_coords(nPl)) <= 2*PLAYER_R) {
                    players[p].c->pos.fx += DT*players[p].c->vel.fx;
                    players[p].c->pos.fy += DT*players[p].c->vel.fy;

                    players[nPl].c->pos.fx += DT*players[nPl].c->vel.fx;
                    players[nPl].c->pos.fy += DT*players[nPl].c->vel.fy;
                }
            }
        }
    }
}

void apply_friction(int nPl) {
    players[nPl].c->vel.fx *= (1.0 - FRICTION*DT);
    players[nPl].c->vel.fy *= (1.0 - FRICTION*DT);
}

double addVX = 0.0;
double addVY = 0.0;
void addEnviroSpeed(int nPl) {
    car* c = players[nPl].c;

    // Wind
    if(WIND_X*WIND_X + WIND_Y*WIND_Y >= 0.001) {
        c->vel.fx += WIND_X*DT;
        c->vel.fx += WIND_Y*DT;
    }

    // B
    if(B > 0.001) {
        addVX = c->vel.fy*B;
        addVY = -c->vel.fx*B;

        c->vel.fx += addVX*DT;
        c->vel.fy += addVY*DT;
    }
}

void removeEnviroSpeed(int nPl) {
    // no
}

// -- //

// return true iif the car has moved
bool updateCar(level* lvl, int nPl) {
    if(norm(players[nPl].c->vel) >= DV) {
        switch (lvl->map[players[nPl].c->chy][players[nPl].c->chx]){
            case NONE:
                fprintf(stderr, "[player %d] Is in an empty chunk (at %d %d)\n", nPl, players[nPl].c->chx, players[nPl].c->chy);
                return false;

            case START:
                addEnviroSpeed(nPl);
                move_on_START(nPl);
                removeEnviroSpeed(nPl);

                bumpOtherCars(nPl);
                apply_friction(nPl);
                updateChunk(lvl, nPl);
                return true;

            case END:
                addEnviroSpeed(nPl);
                move_on_FINISH(nPl);
                removeEnviroSpeed(nPl);

                bumpOtherCars(nPl);
                apply_friction(nPl);
                updateChunk(lvl, nPl);
                return true;
                
            case STR_V:
                addEnviroSpeed(nPl);
                move_on_STR_V(nPl);
                removeEnviroSpeed(nPl);

                bumpOtherCars(nPl);
                apply_friction(nPl);
                updateChunk(lvl, nPl);
                return true;

            case STR_H:
                addEnviroSpeed(nPl);
                move_on_STR_H(nPl);
                removeEnviroSpeed(nPl);

                bumpOtherCars(nPl);
                apply_friction(nPl);
                updateChunk(lvl, nPl);
                return true;

            case TURN_NE:
                addEnviroSpeed(nPl);
                move_on_TURN(nPl, ROOM_SIZE, 0);
                removeEnviroSpeed(nPl);

                bumpOtherCars(nPl);
                apply_friction(nPl);
                updateChunk(lvl, nPl);
                return true;

            case TURN_NW:
                addEnviroSpeed(nPl);
                move_on_TURN(nPl, 0, 0);
                removeEnviroSpeed(nPl);

                bumpOtherCars(nPl);
                apply_friction(nPl);
                updateChunk(lvl, nPl);
                return true;

            case TURN_SE:
                addEnviroSpeed(nPl);
                move_on_TURN(nPl, ROOM_SIZE, ROOM_SIZE);
                removeEnviroSpeed(nPl);

                bumpOtherCars(nPl);
                apply_friction(nPl);
                updateChunk(lvl, nPl);
                return true;

            case TURN_SW:
                addEnviroSpeed(nPl);
                move_on_TURN(nPl, 0, ROOM_SIZE);
                removeEnviroSpeed(nPl);

                bumpOtherCars(nPl);
                apply_friction(nPl);
                updateChunk(lvl, nPl);
                return true;
            
            default:
                return false;
        }
    } else {
        return false;
    }
}

void hasWon(int nPl, int* ranks, int* incr) {
    if(0==winners[nPl]) {
        car* c = players[nPl].c;
        winners[nPl] = ((c->chx == FINISH_CHY && c->chy == FINISH_CHX)?(1):(0));
        if(winners[nPl]==1 && ranks[nPl] == nPlayers) {
            ranks[nPl] = nextRank;
            *incr += 1;
            play_sound("sound/audio/kahoot_10s.wav");
        }
        if(winners[nPl]==1 && remainingTurns == -1) {
            remainingTurns = 4;
        }
    }
    
}

void updateWins(int* ranks) {
    int incr = 0;
    for(int p = 0; p < nPlayers; p++) {
        hasWon(p, ranks, &incr);
    }
    nextRank += incr;
}

// return true if at least one car moved
bool updateCars(level* lvl) {
    bool res = false;
    for(int p = 0; p < nPlayers; p++) {
        if(0==winners[p]) {
            bool rr = updateCar(lvl, p);
            res = res || rr;
        }
    }
    return res;
}