alalala

#pragma once

#include "pch.h"
#include "MeshObject.h"
#include "Quat.h"

using namespace Kore;

// A plane is defined as the plane's normal and the distance of the plane to the origin
class PlaneCollider {
public:

    // Distance
    float d;

    // Normal
    vec3 normal;
};


/************************************************************************/
/* Task P9.2 Implement the box-sphere intersection test you derived for
/* this task. You can use the collider in the main update function in Exercise.cpp
/* to test it against the sphere (you can find it via phyics.physicsObject[0]).
/* You can instantiate your collider in the Exercise.cpp init function.
/************************************************************************/
class BoxCollider {
public:
    const vec3 center;
    const float length;
    const float height;
    const float depth;

    BoxCollider(vec3 center, float length, float height, float depth)
        : center(center), length(length), height(height), depth(depth) {
    }
};



class TriangleCollider {
    void LoadVector(vec3& v, int index, float* vb) {
        v.set(vb[index*8 + 0], vb[index*8 + 1], vb[index*8 + 2]);
    }

public:
    vec3 A;
    vec3 B;
    vec3 C;

    float Area() {
        return 0.5f * ((B-A).cross(C-A)).getLength();
    }

    void LoadFromBuffers(int index, int* ib, float* vb) {
        LoadVector(A, ib[index * 3], vb);
        LoadVector(B, ib[index * 3 + 1], vb);
        LoadVector(C, ib[index * 3 + 2], vb);
    }

    vec3 GetNormal() {
        vec3 n = (B-A).cross(C-A);
        n.normalize();
        return n;
    }

    PlaneCollider GetPlane() {
        vec3 n = GetNormal();
        float d = n.dot(A);

        PlaneCollider p;
        p.d = -d;
        p.normal = n;
        return p;
    }
};

class TriangleMeshCollider {
public:
    MeshObject* mesh;

    int lastCollision;
};

// A sphere is defined by a radius and a center.
class SphereCollider {
public:

    vec3 center;
    float radius;

    // Return true iff there is an intersection with the other sphere
    bool IntersectsWith(const SphereCollider& other) {
        float distance = (other.center - center).getLength();
        return distance < other.radius + radius;
    }

    // Collision normal is the normal vector pointing towards the other sphere
    vec3 GetCollisionNormal(const SphereCollider& other) {
        vec3 n = other.center - center;
        n = n.normalize();
        return n;
    }

    // The penetration depth
    float PenetrationDepth(const SphereCollider& other) {
        return other.radius + radius - (other.center - center).getLength();
    }

    vec3 GetCollisionNormal(const PlaneCollider& other) {
        return other.normal;
    }

    float PenetrationDepth(const PlaneCollider &other) {
        return other.normal.dot(center) + other.d - radius;
    }

    float Distance(const PlaneCollider& other) {
        return other.normal.dot(center) + other.d;
    }

    bool IntersectsWith(const PlaneCollider& other) {
        return Kore::abs(Distance(other)) <= radius;
    }

    bool IsInside(const PlaneCollider& other) {
        return Distance(other) < radius;
    }

    bool IsOutside(const PlaneCollider& other) {
        return Distance(other) > radius;
    }

    /************************************************************************/
    /* Task P9.2 - Check for intersection with a box collider            */
    /************************************************************************/
    bool IntersectsWith(const BoxCollider& other) {
        vec3 closestPoint = this->closestPoint(other);

        if (center.distance(closestPoint) <= radius)
            return true;

        // else there is no intersection
        return false;
    }

    vec3 GetCollisionNormal(const TriangleCollider& other) {
        // Use the normal of the triangle plane
        vec3 n = (other.B - other.A).cross(other.C - other.A);
        n.normalize();
        return n;
    }

    float PenetrationDepth(const TriangleCollider& other) {
        // Use the triangle plane
        // The assumption is that we have a mesh and will not collide only on a vertex or an edge

        return 0.0f;
    }

    bool IntersectsWith(TriangleMeshCollider& other) {
        TriangleCollider coll;
        int* current = other.mesh->mesh->indices;
        float* currentVertex = other.mesh->mesh->vertices;
        for (int i = 0; i < other.mesh->mesh->numFaces; i++) {
            coll.LoadFromBuffers(i, current, currentVertex);
            if (coll.Area() < 0.1f) continue;
            if (IntersectsWith(coll)) {
                other.lastCollision = i;
                vec3 normal;
                if (coll.GetNormal().x() < -0.8f)
                    normal = coll.GetNormal();
                // Kore::log(Warning, "Intersected with triangle: %f, %f, %f", coll.GetNormal().x(), coll.GetNormal().y(), coll.GetNormal().z());
                return true;
            }
        }
        return false;
    }

    vec3 GetCollisionNormal(const TriangleMeshCollider& other) {
        TriangleCollider coll;
        coll.LoadFromBuffers(other.lastCollision, other.mesh->mesh->indices, other.mesh->mesh->vertices);
        return coll.GetNormal();
    }

    float PenetrationDepth(const TriangleMeshCollider& other) {
        // Get a collider for the plane of the triangle
        TriangleCollider coll;
        coll.LoadFromBuffers(other.lastCollision, other.mesh->mesh->indices, other.mesh->mesh->vertices);
        PlaneCollider plane = coll.GetPlane();

        return PenetrationDepth(plane);
    }


    // Find the point where the sphere collided with the triangle mesh
    vec3 GetCollisionPoint(const TriangleMeshCollider& other) {
        // We take the point on the sphere that is closest to the triangle
        vec3 result = center - GetCollisionNormal(other) * radius;
        return result;
    }

    // Find a set of basis vectors such that the provided collision normal x is the first column of the basis matrix,
    // and the other two vectors are perpendicular to the collision normal
    mat3 GetCollisonBasis(vec3 x) {
        x.normalize();
        mat3 basis;
        basis.Set(0, 0, x.x());
        basis.Set(1, 0, x.y());
        basis.Set(2, 0, x.z());

        // Find a y-vector
        // x will often be the global y-axis, so don't use this -> use global z instead
        vec3 y(0, 0, 1);
        y = x.cross(y);
        y = y.normalize();

        basis.Set(0, 1, y.x());
        basis.Set(1, 1, y.y());
        basis.Set(2, 1, y.z());

        vec3 z = x.cross(y);
        z.normalize();

        basis.Set(0, 2, z.x());
        basis.Set(1, 2, z.y());
        basis.Set(2, 2, z.z());
        return basis;
    }

    /************************************************************************/
    /* Task P9.1: Implement these functions. They should return true only if the
     * axis from the sphere center to the vertex of the triangle is a separating axis.
    /************************************************************************/
    bool IsSeparatedByVertexA(const TriangleCollider& other)
    {
        return isSeparatedByVertex(other, other.A);
    }

    bool IsSeparatedByVertexB(const TriangleCollider& other)
    {
        return isSeparatedByVertex(other, other.B);
    }

    bool IsSeparatedByVertexC(const TriangleCollider& other)
    {
        return isSeparatedByVertex(other, other.C);
    }

    bool IntersectsWith(const TriangleCollider& other) {
        // Separating Axes Test
        // 1 Face
        // 3 Vertices
        // 3 Edges
        // No overlap if all of the resulting axes are not touching

        vec3 A = other.A - center;
        vec3 B = other.B - center;
        vec3 C = other.C - center;
        float rr = radius * radius;
        vec3 V = (B - A).cross(C - A);
        float d = A.dot(V);
        float e = V.dot(V);
        bool sepPlane = d * d > rr * e;
        float aa = A.dot(A);
        float ab = A.dot(B);
        float ac = A.dot(C);
        float bb = B.dot(B);
        float bc = B.dot(C);
        float cc = C.dot(C);
        vec3 AB = B - A;
        vec3 BC = C - B;
        vec3 CA = A - C;
        float d1 = ab - aa;
        float d2 = bc - bb;
        float d3 = ac - cc;
        float e1 = AB.dot(AB);
        float e2 = BC.dot(BC);
        float e3 = CA.dot(CA);
        vec3 Q1 = A * e1 - d1 * AB;
        vec3 Q2 = B * e2 - d2 * BC;
        vec3 Q3 = C * e3 - d3 * CA;
        vec3 QC = C * e1 - Q1;
        vec3 QA = A * e2 - Q2;
        vec3 QB = B * e3 - Q3;
        bool sepEdge1 = (Q1.dot(Q1) > rr * e1 * e1) & (Q1.dot(QC) > 0);
        bool sepEdge2 = (Q2.dot(Q2) > rr * e2 * e2) & (Q2.dot(QA) > 0);
        bool sepEdge3 = (Q3.dot(Q3) > rr * e3 * e3) & (Q3.dot(QB) > 0);

        bool sepVertexA = IsSeparatedByVertexA(other);
        bool sepVertexB = IsSeparatedByVertexB(other);
        bool sepVertexC = IsSeparatedByVertexC(other);

        bool separated = sepPlane | sepVertexA | sepVertexB | sepVertexC | sepEdge1 | sepEdge2 | sepEdge3;

        return !separated;
    }


private:
    // for P9.1
    bool isSeparatedByVertex(const TriangleCollider& other, vec3 vertex) {

        using Kore::pow;

        // 1. distance from vertex (A, B, or C) to sphere's center (P) is greater than the sphere's radius
        vec3 diff = center - vertex;
        float relative_dist = pow(diff.x(), 2) + pow(diff.y(), 2) + pow(diff.z(), 2);
        bool distance_greater_radius = relative_dist > pow(radius, 2);

        // 2. the neighbor's of 'vertex' lie on the opposite direction of the sphere's center (P)
            // note: two vectors (v and w) point in the opposite direction if the angle between them is > 90°
            // ... this is the case when dot(v, w) < 0
            // (> 0 implies same direction)

        // calculate vectors which point from 'vertex' to P and its two neighbors; then calculate the angle between them
        vec3 vertex_to_center = center - vertex;    // AP, BP, or CP
        vec3 vertex_to_neighbor1;
        vec3 vertex_to_neighbor2;

        if (vertex == other.A) {
            vertex_to_neighbor1 = other.B - vertex; // AB
            vertex_to_neighbor2 = other.C - vertex; // AC
        } else if (vertex == other.B) {
            vertex_to_neighbor1 = other.A - vertex; // BA
            vertex_to_neighbor2 = other.C - vertex; // BC
        } else if (vertex == other.C) {
            vertex_to_neighbor1 = other.A - vertex; // CA
            vertex_to_neighbor2 = other.B - vertex; // CB
        } else {
            throw std::runtime_error("Supplied vertex does not match any vertex of the triangle in question.");
        }

        bool isOpposite = vertex_to_center.dot(vertex_to_neighbor1) < 0 && vertex_to_center.dot(vertex_to_neighbor2) < 0;

        return distance_greater_radius && isOpposite;
    }

    // for P9.2
    vec3 closestPoint(BoxCollider other) {

        // figure out min/max coordinates of x-, y- and z-directions of our box
        float length_half   = other.length / 2;
        float height_half   = other.height / 2;
        float depth_half    = other.depth / 2;

        float min_x = other.center.x() - length_half;
        float max_x = other.center.x() + length_half;
        float max_y = other.center.y() + height_half;
        float min_y = other.center.y() - height_half;
        float max_z = other.center.z() + depth_half;
        float min_z = other.center.z() - depth_half;

        // the coordinates of the closest point
        // the sphere's center-coordinates are simply clamped to the box (AABB)
        float x;
        float y;
        float z;

        // x-coordinate
        if (center.x() < min_x) {
            x = min_x;
        } else if (center.x() > max_x) {
            x = max_x;
        } else {
            x = center.x();
        }

        // y-coordinate
        if (center.y() < min_y) {
            y = min_y;
        }
        else if (center.y() > max_y) {
            y = max_y;
        }
        else {
            y = center.y();
        }

        // z-coordinate
        if (center.z() < min_z) {
            z = min_z;
        } else if (center.z() > max_z) {
            z = max_z;
        } else {
            z = center.z();
        }

        return vec3(x,y,z);
    }
};