#include <algorithm>
#include <vector>
#include <iostream>
#include <limits>
#include <stdlib.h>

#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>



#define GRAVITY -9.81f
#define DRAG_COEFFICIENT 0.1f /*(i made it ui made it up)*/
#define AIR_DENSITY 1.2
#define DEFAULT_ELASTICITY 0.3f
#define DEFAULT_FRICTION 0.2f

#define COLLIDER_VERTEX_COUNT 8

struct OverlapInfo{
    bool isOverlapping;
    float overlap;
};

class Body{
public:
    Body() = default;
    Body(glm::vec3 pos, glm::vec3 size, bool isStatic);
    void CheckCollision(Body* other);

    void Update();
    glm::vec3 pos;
    glm::vec3 vel;
    glm::vec3 size;

    bool isStatic;
    bool isGrounded;

    glm::vec3 points[COLLIDER_VERTEX_COUNT];
    glm::vec3 vertices[COLLIDER_VERTEX_COUNT];
private:

    void UpdateVertices();

    glm::mat4 model;

    static OverlapInfo CheckOverlap(Body* b1, Body* b2, glm::vec3 axis);
    static float CalculateOverlap(float aMinProj, float aMaxProj, float bMinProj, float bMaxProj);


    static std::vector<Body*> bodies;

};
Body::Body(glm::vec3 pos, glm::vec3 size, bool isStatic){
    this->pos = pos;
    this->size = size;
    this->isStatic = isStatic;
    this->vel = glm::vec3(0);



    isGrounded = false;

    //dont do this each time fix sometime
    points[0] = glm::vec3(-0.5f, -0.5f, -0.5f);
    points[1] = glm::vec3(-0.5f, -0.5f, 0.5f);
    points[2] = glm::vec3(-0.5f, 0.5f, -0.5f);
    points[3] = glm::vec3(-0.5f, 0.5f, 0.5f);
    points[4] = glm::vec3(0.5f, -0.5f, -0.5f);
    points[5] = glm::vec3(0.5f, -0.5f, 0.5f);
    points[6] = glm::vec3(0.5f, 0.5f, -0.5f);
    points[7] = glm::vec3(0.5f, 0.5f, 0.5f);

    UpdateVertices();


    model = glm::mat4(1.0f);
}

void Body::UpdateVertices(){
    model = glm::mat4(1.0f);
    model = glm::translate(model, pos);
    model = glm::scale(model, size * 2.0f);

    for(uint32_t i = 0; i < COLLIDER_VERTEX_COUNT; i++){
        glm::vec4 v = model*glm::vec4(points[i], 1.0f);
        vertices[i] = glm::vec3(v.x, v.y, v.z);
    }
}

void Body::Update(){
    if(isStatic){
        return;
    }

    pos += vel;

    UpdateVertices();


    //do it in a global func or smthn
    isGrounded = false;
}
float Body::CalculateOverlap(float aMinProj, float aMaxProj, float bMinProj, float bMaxProj){
    return std::min(bMaxProj, aMaxProj) - std::max(aMinProj, bMinProj);
}

void Body::CheckCollision(Body* other){

    if(isStatic && other->isStatic){
        return;
    }

    glm::vec3 axis;

    //calculate the fucking normal axis:

    glm::vec3 mtvAxis;
    float overlap = std::numeric_limits<float>::infinity();

    for(uint32_t i = 0; i < COLLIDER_VERTEX_COUNT; i++){
        glm::vec3 curr = vertices[i];
        glm::vec3 edges[2];

        edges[0] = vertices[(i + 1) % COLLIDER_VERTEX_COUNT]- curr;
        edges[1] = vertices[(i + 2) % COLLIDER_VERTEX_COUNT]- curr;

        axis = glm::normalize(glm::cross(edges[1], edges[0]));

        OverlapInfo overlapInfo = CheckOverlap(this, other, axis);
        if(!overlapInfo.isOverlapping){
            return;
        }
        if(overlapInfo.overlap < overlap){
            overlap = overlapInfo.overlap;
            mtvAxis = axis;
        }

        curr = other->vertices[i];
        edges[0] = other->vertices[(i + 1) % COLLIDER_VERTEX_COUNT]- curr;
        edges[1] = other->vertices[(i + 2) % COLLIDER_VERTEX_COUNT]- curr;

        axis = glm::normalize(glm::cross(edges[1], edges[0]));

        overlapInfo = CheckOverlap(this, other, axis);
        if(!overlapInfo.isOverlapping){
            return;
        }
        if(overlapInfo.overlap < overlap){
            overlap = overlapInfo.overlap;
            mtvAxis = axis;
        }

        axis = glm::normalize(glm::cross(vertices[(i + 1) % COLLIDER_VERTEX_COUNT] - curr,
        other->vertices[(i + 1) % COLLIDER_VERTEX_COUNT] - other->vertices[i]));

        overlapInfo = CheckOverlap(this, other, axis);
        if(!overlapInfo.isOverlapping){
            return;
        }
        if(overlapInfo.overlap < overlap){
            overlap = overlapInfo.overlap;
            mtvAxis = axis;
        }

        axis = glm::normalize(glm::cross(vertices[(i + 2) % COLLIDER_VERTEX_COUNT] - curr,
        other->vertices[(i + 2) % COLLIDER_VERTEX_COUNT] - other->vertices[i]));

        overlapInfo = CheckOverlap(this, other, axis);
        if(!overlapInfo.isOverlapping){
            return;
        }

        if(overlapInfo.overlap < overlap){
            overlap = overlapInfo.overlap;
            mtvAxis = axis;
        }

    }

    mtvAxis.x = static_cast<float>(static_cast<int>(mtvAxis.x * 10)) / 10;
    mtvAxis.y = static_cast<float>(static_cast<int>(mtvAxis.y * 10)) / 10;
    mtvAxis.z = static_cast<float>(static_cast<int>(mtvAxis.z * 10)) / 10;


    pos -= mtvAxis * overlap;

    vel.y = 0;

}

OverlapInfo Body::CheckOverlap(Body* b1, Body* b2, glm::vec3 axis){
    OverlapInfo overlapInfo;

    float aMaxProj = -std::numeric_limits<float>::infinity();
    float aMinProj = std::numeric_limits<float>::infinity();

    float bMaxProj = -std::numeric_limits<float>::infinity();
    float bMinProj = std::numeric_limits<float>::infinity();


    for(glm::vec3 p : b1->vertices){
        float proj = glm::dot(axis, p);

        if(proj < aMinProj){
            aMinProj = proj;
        }
        if(proj > aMaxProj){
            aMaxProj = proj;
        }
    }

    for(glm::vec3 p : b2->vertices){
        float proj = glm::dot(axis, p);

        if(proj < bMinProj){
            bMinProj = proj;
        }
        if(proj > bMaxProj){
            bMaxProj = proj;
        }
    }
    if(aMaxProj < bMinProj || bMaxProj < aMinProj){
        overlapInfo.isOverlapping = false;
        overlapInfo.overlap = std::numeric_limits<float>::infinity();
        return overlapInfo;
    }

    overlapInfo.isOverlapping = true;
    overlapInfo.overlap = CalculateOverlap(aMinProj, aMaxProj, bMinProj, bMaxProj);
    return overlapInfo;
}

int main(){
    Body body = Body(glm::vec3(0), glm::vec3(1, 2.5, 1), false);
    Body body3 = Body(glm::vec3(3, 0, 3), glm::vec3(1, 5, 1), true);

    while(true){

        std::string input;
        std::cin >> input;

        if(!input.compare("-x")){
            body.pos.x -= 1;
        }
        else if(!input.compare("+x")){
            body.pos.x += 1;
        }
        else if(!input.compare("-z")){
            body.pos.z -= 1;
        }
        else if(!input.compare("+z")){
            body.pos.z += 1;
        }

        body.CheckCollision(&body3);

        body.Update();

        std::cout << body.pos.x << " " << body.pos.z << std::endl;
    }
}