//
// Created by Micha on 27/09/2021.
//

#pragma once

#include "../WallType.h"

#include <cmath>
#include <glm/glm.hpp>
#include <glm/gtx/intersect.hpp>
#include <glm/gtx/perpendicular.hpp>

static inline bool LineSegmentIntersections(
    glm::vec2 originOne,  // Line 1 start
    glm::vec2 endOne,     // Line 1 end
    glm::vec2 originTwo,  // Line 2 start
    glm::vec2 endTwo,     // Line 2 end
    glm::vec2& intersectPoint,
    float& distance)  // Output
{
    // Line AB represented as a1x + b1y = c1
    float a1 = endOne.y - originOne.y;
    float b1 = originOne.x - endOne.x;
    float c1 = a1 * (originOne.x) + b1 * (originOne.y);

    // Line CD represented as a2x + b2y = c2
    float a2 = endTwo.y - originTwo.y;
    float b2 = originTwo.x - endTwo.x;
    float c2 = a2 * (originTwo.x) + b2 * (originTwo.y);

    float determinant = a1 * b2 - a2 * b1;

    if (determinant == 0) {
        // The lines are parallel. This is simplified
        // by returning a pair of FLT_MAX
        intersectPoint.x = NAN;
        intersectPoint.y = NAN;
        return false;
    } else {
        intersectPoint.x = (b2 * c1 - b1 * c2) / determinant;
        intersectPoint.y = (a1 * c2 - a2 * c1) / determinant;
        distance = glm::length(intersectPoint - originOne);
        if (distance < glm::length(endOne - originOne))
            return true;
        else
            return false;
    }
}

//----------------------- doWallsObstructLineSegment --------------------------
//
//  given a line segment defined by the points from and to, iterate through all
//  the map objects and walls and test for any intersection. This method
//  returns true if an intersection occurs.
//-----------------------------------------------------------------------------

template <class ContWall>
inline bool doWallsObstructLineSegment(glm::vec2 from,
                                       glm::vec2 to,
                                       const ContWall& walls) {
    // test against the walls
    // ContWall::const_iterator curWall = walls.begin();
    float distance;
    for (auto& curWall : walls) {
        // do a line segment intersection test
        glm::vec2 intersectPoint(.0f);
        glm::vec2 normal(glm::normalize(to - from));
        if (LineSegmentIntersections(from,
                                     to,
                                     curWall.getOriginPoint(),
                                     curWall.getEndPoint(),
                                     intersectPoint,
                                     distance)) {
            if (curWall.getOriginPoint().x == curWall.getEndPoint().x) {
                if (intersectPoint.y < curWall.getEndPoint().y
                    && intersectPoint.y > curWall.getOriginPoint().y) {
                    return false;
                }
            } else {
                if (intersectPoint.x < curWall.getEndPoint().x
                    && intersectPoint.x > curWall.getOriginPoint().x) {
                    return false;
                }
            }
        }
    }
    return true;
}

//----------------------- doWallsObstructCylinderSides -------------------------
//
//  similar to above except this version checks to see if the sides described
//  by the cylinder of length |AB| with the given radius intersect any walls.
//  (this enables the trace to take into account any the bounding radii of
//  entity objects)
//-----------------------------------------------------------------------------
template <class ContWall>
inline bool doWallsObstructCylinderSides(glm::vec2 A,
                                         glm::vec2 B,
                                         float BoundingRadius,
                                         const ContWall& walls) {
    // the line segments that make up the sides of the cylinder must be created
    glm::vec2 toB = glm::normalize(B - A);

    // A1B1 will be one side of the cylinder, A2B2 the other.
    glm::vec2 A1, B1, A2, B2;

    // glm::perp(glm::vec2(0, 0), m_HeadingVec);
    // glm::vec2 radialEdge = toB.Perp() * BoundingRadius;
    glm::vec2 radialEdge = glm::perp(glm::vec2(0, 0), toB) * BoundingRadius;

    // create the two sides of the cylinder
    A1 = A + radialEdge;
    B1 = B + radialEdge;

    A2 = A - radialEdge;
    B2 = B - radialEdge;

    // now test against them
    if (!doWallsObstructLineSegment(A1, B1, walls)) {
        return doWallsObstructLineSegment(A2, B2, walls);
    }

    return true;
}

//------------------------ doWallsIntersectCircle -----------------------------
//
//  returns true if any walls intersect the circle of radius at point p
//-----------------------------------------------------------------------------
template <class ContWall>
inline bool doWallsIntersectCircle(const ContWall& walls,
                                   glm::vec2 point,
                                   float radius) {
    // test against the walls
    for (auto& curWall : walls) {
        // Unused variables
        glm::vec2 a, b, c, d;
        if (glm::intersectLineSphere(curWall.getOriginPoint(),
                                     curWall.getEndPoint(),
                                     point,
                                     radius,
                                     a,
                                     b,
                                     c,
                                     d)) {
            return true;
        }
    }

    return false;
}

//------------------ FindClosestPointOfIntersectionWithWalls ------------------
//
//  tests a line segment against the container of walls  to calculate
//  the closest intersection point, which is stored in the reference 'ip'. The
//  distance to the point is assigned to the reference 'distance'
//
//  returns false if no intersection point found
//-----------------------------------------------------------------------------

template <class ContWall>
inline bool FindClosestPointOfIntersectionWithWalls(glm::vec2 A,
                                                    glm::vec2 B,
                                                    float& distance,
                                                    glm::vec2& ip,
                                                    const ContWall& walls) {
    distance = (std::numeric_limits<float>::max)();

    // ContWall::const_iterator curWall = walls.begin();
    for (auto& curWall : walls) {
        glm::vec2 point;
        float newDistance{0};
        if (LineSegmentIntersections(A,
                                     B,
                                     curWall.getOriginPoint(),
                                     curWall.getEndPoint(),
                                     point,
                                     newDistance)) {
            distance = glm::length(point - A);
            if (newDistance < distance) {
                distance = newDistance;
                ip = point;
            }
        }
    }

    if (distance < (std::numeric_limits<float>::max)())
        return true;

    return false;
}

/// Calculate intersection of two lines.
/// return true if found, false if not found or error

static inline float determinate(float a, float b, float c, float d) {
    return a * d - b * c;
}

/*
static inline bool LineSegmentIntersections(glm::vec2 originOne, // Line 1 start
                       glm::vec2 endOne, // Line 1 end
                       glm::vec2 originTwo, // Line 2 start
                       glm::vec2 endTwo, // Line 2 end
                       glm::vec2 &intersectPoint,
                        float &distance) // Output
                       {
    glm::vec2 lineIntersectPoint(0.0f);
    float detL1 = determinate(originOne.x, originOne.y, endOne.x, endOne.y);
    float detL2 = determinate(originTwo.x, originTwo.y , endTwo.x, endTwo.y);
    float x1mx2 = originOne.x - endOne.x;
    float x3mx4 = originTwo.x - endTwo.x;
    float y1my2 = originOne.y - endOne.y;
    float y3my4 = originTwo.y - endTwo.y;

    //float denom = Det(x1mx2, y1my2, x3mx4, y3my4);
    float denom = determinate(x1mx2, y1my2, x3mx4, y3my4);
    if(denom == 0.0) // Lines don't seem to cross
    {
        intersectPoint = glm::vec2(NAN, NAN);
        return false;
    }

    //float xnom = Det(detL1, x1mx2, detL2, x3mx4);
    //float ynom = Det(detL1, y1my2, detL2, y3my4);
    float xnom = determinate(detL1, x1mx2, detL2, x3mx4);
    float ynom = determinate(detL2, y1my2, detL2, y3my4);

    auto xMinMax = std::minmax({originOne.x, endOne.x});//, originTwo.x,
endTwo.x}); auto yMinMax = std::minmax({originOne.y, originOne.y});//,
originTwo.y, endTwo.y});

    lineIntersectPoint.x = xnom / denom;
    lineIntersectPoint.y = ynom / denom;
    if(!isfinite(lineIntersectPoint.x) || !isfinite(lineIntersectPoint.y)) //
Probably a numerical issue return false;

    distance = glm::length(lineIntersectPoint - originOne);

    if(distance < glm::length(endOne - originOne)){
        intersectPoint = lineIntersectPoint;
        return true;    //line segments intersect
    }

    if(xMinMax.x < lineIntersectPoint.x && lineIntersectPoint.x < xMinMax.y &&
        yMinMax.x < lineIntersectPoint.y && lineIntersectPoint.y < yMinMax.y){
        intersectPoint = lineIntersectPoint;
        return true;    //line segments intersect
    }*//*
    else{
        intersectPoint = glm::vec2(NAN, NAN);
        return false; //lines intersect but segments dont
    }
}*/

// ICT290_WALLINTERSECTIONTESTS_H