#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 "hash.h"
#include "base.h"

int ln_baseN(int n, int b) {
    int r = 0;
    while(n != 0) {
        n = n / b;
        r++;
    }
    return r;
}

int pw(int x, int n) {
    if (n<0)
        return 0;
    if (n==0)
        return 1;
    if (n%2==0)
        return pw(x*x, n/2);
    return x*pw(x*x, n/2);
}

double pwf(double x, int n) {
    if (n<0)
        return 0.0;
    if (n==0)
        return 1.0;
    if (n%2==0)
        return pwf(x*x, n/2);
    return x*pwf(x*x, n/2);
}

int abs(int n) {
    if(n > 0) {
        return n;
    } else {
        return (-n);
    }
}

int min(int a, int b) {
    if(a > b) {
        return b;
    };
    return a;
}

int max(int a, int b) {
    if(a < b) {
        return b;
    };
    return a;
}

float minf(float a, float b) {
    if(a > b) {
        return b;
    };
    return a;
}

float maxf(float a, float b) {
    if(a < b) {
        return b;
    };
    return a;
}

double mind(double a, double b) {
    if(a > b) {
        return b;
    };
    return a;
}

double maxd(double a, double b) {
    if(a < b) {
        return b;
    };
    return a;
}

double absf(double n) {
    if(n > 0.0f) {
        return n;
    } else {
        return (-n);
    }
}

bool xor(bool a, bool b) {
    return ((a && !b) || (!a && b));
}

// n >= 0
int reverse(int n) {
    int res = 0;
    int nexp = pw(10, ln_baseN(n, 10));
    int cexp = nexp/10;
    int left = n;
    while(cexp > 0) {
        res += (left%10)*cexp;
        cexp /= 10;
        left /= 10;
    }
    //printf("%d -> %d\n", n, res);
    return res;
}

int convex_seg(int x1, int x2, double theta) {
    return (int)(((1.0f - theta) * x1 + theta * x2));
}

double convex_pt(double a, double b, double theta) {
    return (a+(b-a)*theta);
}

double convex_tri(double a, double tha, double b, double thb, double c, double thc) {
    return (a*tha + b*thb + c*thc);
}

pt_2d convex_pt2d(pt_2d A, pt_2d B, double theta) {
    pt_2d res;
    res.x = A.x+(B.x-A.x)*theta;
    res.y = A.y+(B.y-A.y)*theta;
    res.z = A.z+(B.z-A.z)*theta;
    return res;
}

pt_2d convex_pt2d_tri(pt_2d A, double tha, pt_2d B, double thb, pt_2d C, double thc) {
    pt_2d res;
    res.x = A.x*tha+B.x*thb+C.x*thc;
    res.y = A.y*tha+B.y*thb+C.y*thc;
    res.z = A.z*tha+B.z*thb+C.z*thc;
    return res;
}

bool is_an_integer(char c) {
    return ((int)c >= 48 && (int)c <= 57);
}

pt_2d vect_diff(pt_2d p1, pt_2d p2) {
    pt_2d res;
    res.x = p2.x - p1.x;
    res.y = p2.y - p1.y;
    res.z = p2.z - p1.z;
    return res;
}

double dot2D(pt_2d p1, pt_2d p2) {
    return p1.x * p2.x + p1.y * p2.y;
}

double dot3D(pt_2d p1, pt_2d p2) {
    return p1.x * p2.x + p1.y * p2.y + p1.z * p2.z;
}

double to_double(int n) {
    return (double)n;
}

int to_int(double n) {
    return (int)n;
}

int line_count(char* filename) {
    FILE* ptr = fopen(filename, "r");
    char c = 'd';

    int n = 0;
    while(c != EOF) {
        if(c == '\n') {
            n += 1;
        };
        c = fgetc(ptr);
    };
    fclose(ptr);
    return (n+1);
}

int str_to_int(char* s) {
    int res = 0;
    int i = 0;
    while(s[i] != '\0' && is_an_integer(s[i])) {
        res *= 10;
        res += (int)s[i] - 48;
        i++;
    };
    return res;
}

bool str_equal(char* s1, char* s2) {
    if(s1[0] == '\0' || s2[0] == '\0') {
        return (s1[0] == '\0' && s2[0] == '\0');
    } 
    return (s1[0] == s2[0] && str_equal(&s1[1], &s2[1]));
}

// ------------------------------------------------------------------------------------------------ //

cube_0* create_cube_0(double x, double y, double z, double w, double h, double d, double hz_a, double vt_a, int r, int g, int b) {
    cube_0* cb = malloc(sizeof(cube_0));
    cb->red = r;
    cb->green = g;
    cb->blue = b;
    cb->x = x;
    cb->y = y;
    cb->z = z;
    cb->w = w;
    cb->h = h;
    cb->d = d;
    cb->hz_angle = hz_a;
    cb->vt_angle = vt_a;
    return cb;
}

void fill_cube_0(cube_0* cb, double x, double y, double z, double w, double h, double d, double hz_a, double vt_a, int r, int g, int b) {
    cb->red = r;
    cb->green = g;
    cb->blue = b;
    cb->x = x;
    cb->y = y;
    cb->z = z;
    cb->w = w;
    cb->h = h;
    cb->d = d;
    cb->hz_angle = hz_a;
    cb->vt_angle = vt_a;
}

cube create_cube(double x, double y, double z, double w, double h, double d, double hz_a, double vt_a, int r, int g, int b) {
    cube cb = malloc(sizeof(cube_0));
    cb = create_cube_0(x, y, z, w, h, d, hz_a, vt_a, r, g, b);
    return cb;
}

void free_cube(cube c) {
    free(c);
}

teleporter* create_teleporter(
    double x, double y, double z, double w, double h, double d, double hz_a, double vt_a, int r, int g, int b,
    int chx_dest, int chy_dest, double x_dest, double y_dest, double z_dest
) {
    teleporter* tp = malloc(sizeof(teleporter));
    tp->dest_chx = chx_dest;
    tp->dest_chy = chy_dest;
    tp->dest_x = x_dest;
    tp->dest_y = y_dest;
    tp->dest_z = z_dest;
    tp->hitbox = create_cube_0(x, y, z, w, h, d, hz_a, vt_a, r, g, b);
    return tp;
}

void copy_cube(cube_0* src, cube_0* dest) {
    dest->red = src->red;
    dest->green = src->green;
    dest->blue = src->blue;
    dest->hz_angle = src->hz_angle;
    dest->vt_angle = src->vt_angle;
    dest->x = src->x;
    dest->y = src->y;
    dest->z = src->z;
    dest->w = src->w;
    dest->h = src->h;
    dest->d = src->d;
}

// ------------------------------------------------------------------------------------------------ //

double distance_pt_pt_3d(double x0, double y0, double z0, double x1, double y1, double z1) {
    return sqrt((x1 - x0)*(x1 - x0)+(y1 - y0)*(y1 - y0)+(z1 - z0)*(z1 - z0));
}

double distance_pt_pt_2d_sq(double x0, double y0, double x1, double y1) {
    return (x1 - x0)*(x1 - x0)+(y1 - y0)*(y1 - y0);
}

double distance_pt_pt_3d_sq(double x0, double y0, double z0, double x1, double y1, double z1) {
    return (x1 - x0)*(x1 - x0)+(y1 - y0)*(y1 - y0)+(z1 - z0)*(z1 - z0);
}

double distance_pt_seg_3d(double x, double y, double z, double sx, double sy, double sz, double ex, double ey, double ez) {
    double theta = -(
        ((ex - sx) * (sx - x) + (ey - sy) * (sy - y) + (ez - sz) * (sz - z)) /
        ((ex - sx) * (ex - sx) + (ey - sy) * (ey - sy) + (ez - sz) * (ez - sz))
    );
    if(theta >= 0.0 && theta <= 1.0) {
        return (distance_pt_pt_3d(x, y, z, convex_pt(sx, ex, theta), convex_pt(sy, ey, theta), convex_pt(sz, ez, theta)));
    } else if (theta < 0.0) {
        return (distance_pt_pt_3d(x, y, z, sx, sy, sz));
    } else {
        return (distance_pt_pt_3d(x, y, z, ex, ey, ez));
    }
}

double distance_pt_cube_axis(double coord, double begin, double end) {
    if(coord < begin) {
        return (begin);
    } else if(coord > end) {
        return (end);
    } else {
        return (coord);
    }
}

double distance_pt_cube_aligned_3d(double x0, double y0, double z0, double cx, double cy, double cz, double cw, double ch, double cd) {
    double clx = distance_pt_cube_axis(x0, cx, cx+cw);
    double cly = distance_pt_cube_axis(y0, cy, cy+ch);
    double clz = distance_pt_cube_axis(z0, cz, cz+cd);
    return sqrt((x0-clx)*(x0-clx) + (y0-cly)*(y0-cly) + (z0-clz)*(z0-clz));
}

double distance_pt_cube_aligned_3d_infinite(double x0, double y0, double z0, double cx, double cy, double cz, double cw, double ch, double cd) {
    double clx = distance_pt_cube_axis(x0, cx, cx+cw);
    double cly = distance_pt_cube_axis(y0, cy, cy+ch);
    double clz = distance_pt_cube_axis(z0, cz, cz+cd);
    return maxd(maxd(absf(clx-x0), absf(cly-y0)), absf(clz-z0));
}

double distance_pt_cube_0_3d(double x0, double y0, double z0, cube_0* c) {
    // places the origin at the center of the cube
    double x = x0 - (c->x + c->w/2.0);
    double y = y0 - (c->y + c->h/2.0);
    double z = z0 - (c->z + c->d/2.0);

    // rotate the point : y then x
    double xry = x*cos(c->hz_angle) + z*sin(c->hz_angle);
    double yry = y;
    double zry = z*cos(c->hz_angle) - x*sin(c->hz_angle);

    double xrx = xry;
    double yrx = yry*cos(c->vt_angle) - zry*sin(c->vt_angle);
    double zrx = zry*cos(c->vt_angle) + yry*sin(c->vt_angle);

    // now the cube and pt are aligned, and (0, 0, 0) is at the cube's (bary)center
    return distance_pt_cube_aligned_3d(xrx, yrx, zrx, -c->w/2.0, -c->h/2.0, -c->d/2.0, c->w, c->h, c->d);
}

double distance_pt_cube_0_3d_infinite(double x0, double y0, double z0, cube_0* c) {
    // places the origin at the center of the cube
    double x = x0 - (c->x + c->w/2.0);
    double y = y0 - (c->y + c->h/2.0);
    double z = z0 - (c->z + c->d/2.0);

    // rotate the point : y then x
    double xry = x*cos(c->hz_angle) + z*sin(c->hz_angle);
    double yry = y;
    double zry = z*cos(c->hz_angle) - x*sin(c->hz_angle);

    double xrx = xry;
    double yrx = yry*cos(c->vt_angle) - zry*sin(c->vt_angle);
    double zrx = zry*cos(c->vt_angle) + yry*sin(c->vt_angle);

    // now the cube and pt are aligned, and (0, 0, 0) is at the cube's (bary)center
    return distance_pt_cube_aligned_3d_infinite(xrx, yrx, zrx, -c->w/2.0, -c->h/2.0, -c->d/2.0, c->w, c->h, c->d);
}

// ------------------------------------------------------------------------------------------------ //

void project_to_camera(double x0, double y0, double z0, double* rx, double* ry, double* rz) {
    // align pt to (0, 0, 0)
    double x = x0 - camx;
    double y = y0 - camy;
    double z = z0 - camz;

    // rotate (y)
    double xry = x*cos(rot_hz) - z*sin(rot_hz);
    double yry = y;
    double zry = z*cos(rot_hz) + x*sin(rot_hz);

    // rotate (x)
    if(rx != NULL) {*rx = xry;}
    if(ry != NULL) {*ry = yry*cos(rot_vt) + zry*sin(rot_vt);}
    if(rz != NULL) {*rz = zry*cos(rot_vt) - yry*sin(rot_vt);}
}

void project_to_cube(double x0, double y0, double z0, double* rx, double* ry, double* rz, cube_0* c) {
    // align pt to (0, 0, 0)
    double x = x0 - (c->x + c->w/2.0);
    double y = y0 - (c->y + c->h/2.0);
    double z = z0 - (c->z + c->d/2.0);

    // rotate (y)
    double xry = x*cos(c->hz_angle) - z*sin(c->hz_angle);
    double yry = y;
    double zry = z*cos(c->hz_angle) + x*sin(c->hz_angle);

    // rotate (x)
    if(rx != NULL) {*rx = c->x + c->w/2.0 + xry;}
    if(ry != NULL) {*ry = c->y + c->h/2.0 + yry*cos(c->vt_angle) - zry*sin(c->vt_angle);}
    if(rz != NULL) {*rz = c->z + c->d/2.0 + zry*cos(c->vt_angle) + yry*sin(c->vt_angle);}
}

// ------------------------------------------------------------------------------------------------ //

void remove_entity(entity** arr, int* memlen, int* len, int index) {
    if(*len > 0) {
        entity* temp = arr[index];
        arr[index] = arr[*len -1];
        arr[*len -1] = temp;
        *len -= 1;
    }
}

void add_entity(entity** arr, int* memlen, int* len, entity* ent) {
    if(*memlen == *len) {
        entity** newarr = malloc(sizeof(entity*)*2*(*memlen));
        for(int k = 0; k < *len; k++) {
            newarr[k] = malloc(sizeof(entity));
            newarr[k] = arr[k];
            free(arr[k]);
        }
        free(*arr);
        arr = newarr;
        *memlen *= 2;
    }
    free(arr[*len]);
    arr[*len] = ent;
    *len += 1;
}