/*
 * This file is part of the Colobot: Gold Edition source code
 * Copyright (C) 2001-2023, Daniel Roux, EPSITEC SA & TerranovaTeam
 * http://epsitec.ch; http://colobot.info; http://github.com/colobot
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see http://gnu.org/licenses
 */

/**
 * \file math/geometry.h
 * \brief Math functions related to 3D geometry calculations, transformations, etc.
 */

#pragma once


#include "math/const.h"
#include "math/func.h"

#include <glm/glm.hpp>
#include <glm/ext.hpp>

#include <cmath>
#include <cstdlib>


// Math module namespace
namespace Math
{


//! Returns py up on the line \a a - \a b
inline float MidPoint(const glm::vec2&a, const glm::vec2&b, float px)
{
    if (IsEqual(a.x, b.x))
    {
        if (a.y < b.y)
            return Math::HUGE_NUM;
        else
            return -Math::HUGE_NUM;
    }
    return (b.y-a.y) * (px-a.x) / (b.x-a.x) + a.y;
}

//! Tests whether the point \a p is inside the triangle (\a a,\a b,\a c)
inline bool IsInsideTriangle(glm::vec2 a, glm::vec2 b, glm::vec2 c, glm::vec2 p)
{
    float n, m;

    if ( p.x < a.x && p.x < b.x && p.x < c.x ) return false;
    if ( p.x > a.x && p.x > b.x && p.x > c.x ) return false;
    if ( p.y < a.y && p.y < b.y && p.y < c.y ) return false;
    if ( p.y > a.y && p.y > b.y && p.y > c.y ) return false;

    if ( a.x > b.x ) std::swap(a,b);
    if ( a.x > c.x ) std::swap(a,c);
    if ( c.x < a.x ) std::swap(c,a);
    if ( c.x < b.x ) std::swap(c,b);

    n = MidPoint(a, b, p.x);
    m = MidPoint(a, c, p.x);
    if ( (n>p.y||p.y>m) && (n<p.y||p.y<m) ) return false;

    n = MidPoint(c, b, p.x);
    m = MidPoint(c, a, p.x);
    if ( (n>p.y||p.y>m) && (n<p.y||p.y<m) ) return false;

    return true;
}

//! Rotates a point around a center
/**
 * \param center  center of rotation
 * \param angle   angle [radians] (positive is CCW)
 * \param p       the point to be rotated
 */
inline glm::vec2 RotatePoint(const glm::vec2& center, float angle, const glm::vec2& p)
{
    glm::vec2 a;
    a.x = p.x-center.x;
    a.y = p.y-center.y;

    glm::vec2 b;
    b.x = a.x*cosf(angle) - a.y*sinf(angle);
    b.y = a.x*sinf(angle) + a.y*cosf(angle);

    b.x += center.x;
    b.y += center.y;

    return b;
}

//! Rotates a point around the origin (0,0)
/**
 * \param angle angle [radians] (positive is CCW)
 * \param p     the point to be rotated
 */
inline glm::vec2 RotatePoint(float angle, const glm::vec2&p)
{
    float x = p.x*cosf(angle) - p.y*sinf(angle);
    float y = p.x*sinf(angle) + p.y*cosf(angle);

    return { x, y };
}

//! Rotates a vector (dist, 0)
/**
 * \param angle angle [radians] (positive is CCW)
 * \param dist  distance to origin
 */
inline glm::vec2 RotatePoint(float angle, float dist)
{
    float x = dist*cosf(angle);
    float y = dist*sinf(angle);

    return { x, y };
}

//! Rotates a point around a center on 2D plane
/**
 * \param cx,cy  center of rotation
 * \param angle  angle of rotation [radians] (positive is CCW)
 * \param px,py  point coordinates to rotate
 */
inline void RotatePoint(float cx, float cy, float angle, float &px, float &py)
{
    float ax, ay;

    px -= cx;
    py -= cy;

    ax = px*cosf(angle) - py*sinf(angle);
    ay = px*sinf(angle) + py*cosf(angle);

    px = cx+ax;
    py = cy+ay;
}

//! Rotates a point around a center in space
/**
 * \a angleH is rotation along Y axis (heading) while \a angleV is rotation along X axis (TODO: ?).
 *
 * \param center         center of rotation
 * \param angleH,angleV  rotation angles [radians] (positive is CCW)
 * \param p              the point to be rotated
 */
inline void RotatePoint(const glm::vec3 &center, float angleH, float angleV, glm::vec3 &p)
{
    p.x -= center.x;
    p.y -= center.y;
    p.z -= center.z;

    glm::vec3 b{};
    b.x = p.x*cosf(angleH) - p.z*sinf(angleH);
    b.y = p.z*sinf(angleV) + p.y*cosf(angleV);
    b.z = p.x*sinf(angleH) + p.z*cosf(angleH);

    p = center + b;
}

//! Rotates a point around a center in space
/**
 * The rotation is performed first along Y axis (\a angleH) and then along X axis (\a angleV).
 *
 * \param center         center of rotation
 * \param angleH,angleV  rotation angles [radians] (positive is CCW)
 * \param p              the point to be rotated
 */
inline void RotatePoint2(const glm::vec3 center, float angleH, float angleV, glm::vec3 &p)
{
    p.x -= center.x;
    p.y -= center.y;
    p.z -= center.z;

    glm::vec3 a{};
    a.x = p.x*cosf(angleH) - p.z*sinf(angleH);
    a.y = p.y;
    a.z = p.x*sinf(angleH) + p.z*cosf(angleH);

    glm::vec3 b{};
    b.x = a.x;
    b.y = a.z*sinf(angleV) + a.y*cosf(angleV);
    b.z = a.z*cosf(angleV) - a.y*sinf(angleV);

    p = center + b;
}

//! Returns the angle between point (x,y) and (0,0)
inline float RotateAngle(float x, float y)
{
    if (x == 0.0f && y == 0.0f) return 0.0f;

    if (x >= 0.0f)
    {
        if (y >= 0.0f)
        {
            if (x > y) return atanf(y/x);
            else       return PI*0.5f - atanf(x/y);
        }
        else
        {
            if (x > -y) return PI*2.0f + atanf(y/x);
            else        return PI*1.5f - atanf(x/y);
        }
    }
    else
    {
        if (y >= 0.0f)
        {
            if (-x > y) return PI*1.0f + atanf(y/x);
            else        return PI*0.5f - atanf(x/y);
        }
        else
        {
            if (-x > -y) return PI*1.0f + atanf(y/x);
            else         return PI*1.5f - atanf(x/y);
        }
    }
}

//! Calculates the angle between two points and a center
/**
 * \param center  the center point
 * \param p1,p2   the two points
 * \returns       the angle [radians] (positive is CCW)
 */
inline float RotateAngle(const glm::vec2&center, const glm::vec2&p1, const glm::vec2&p2)
{
    if (glm::all(glm::epsilonEqual(p1, center, TOLERANCE)))
        return 0;

    if (glm::all(glm::epsilonEqual(p2, center, TOLERANCE)))
        return 0;

    float a1 = asinf((p1.y - center.y) / glm::distance(p1, center));
    float a2 = asinf((p2.y - center.y) / glm::distance(p2, center));

    if (p1.x < center.x) a1 = PI - a1;
    if (p2.x < center.x) a2 = PI - a2;

    float a = a2 - a1;
    if (a < 0)
        a += 2.0f*PI;

    return a;
}

//! Loads view matrix from the given vectors
/**
 * \param mat      result matrix
 * \param from     origin
 * \param at       view direction
 * \param worldUp  up vector
 */
inline void LoadViewMatrix(glm::mat4 &mat, const glm::vec3 &from,
                           const glm::vec3 &at, const glm::vec3 &worldUp)
{
    // Get the z basis vector, which points straight ahead. This is the
    // difference from the eyepoint to the lookat point.
    glm::vec3 view = at - from;

    float length = glm::length(view);
    assert(! IsZero(length) );

    // Normalize the z basis vector
    view /= length;

    // Get the dot product, and calculate the projection of the z basis
    // vector onto the up vector. The projection is the y basis vector.
    float dotProduct = glm::dot(worldUp, view);

    glm::vec3 up = worldUp - dotProduct * view;

    // If this vector has near-zero length because the input specified a
    // bogus up vector, let's try a default up vector
    if ( IsZero(length = glm::length(up)) )
    {
        up = glm::vec3(0.0f, 1.0f, 0.0f) - view.y * view;

        // If we still have near-zero length, resort to a different axis.
        if ( IsZero(length = glm::length(up)) )
        {
            up = glm::vec3(0.0f, 0.0f, 1.0f) - view.z * view;

            assert(! IsZero(glm::length(up)) );
        }
    }

    // Normalize the y basis vector
    up /= length;

    // The x basis vector is found simply with the cross product of the y
    // and z basis vectors
    glm::vec3 right = glm::cross(up, view);

    // Start building the matrix. The first three rows contains the basis
    // vectors used to rotate the view to point at the lookat point
    mat = glm::mat4(1.0f);

    mat[0][0] = right.x;
    mat[0][1] = up.x;
    mat[0][2] = view.x;
    mat[1][0] = right.y;
    mat[1][1] = up.y;
    mat[1][2] = view.y;
    mat[2][0] = right.z;
    mat[2][1] = up.z;
    mat[2][2] = view.z;

    // Do the translation values (rotations are still about the eyepoint)
    mat[3][0] = -glm::dot(from, right);
    mat[3][1] = -glm::dot(from, up);
    mat[3][2] = -glm::dot(from, view);
}

//! Loads a perspective projection matrix
/**
 * \param mat        result matrix
 * \param fov        field of view in radians
 * \param aspect     aspect ratio (width / height)
 * \param nearPlane  distance to near cut plane
 * \param farPlane   distance to far cut plane
 */
inline void LoadProjectionMatrix(glm::mat4 &mat, float fov = Math::PI / 2.0f, float aspect = 1.0f,
                                 float nearPlane = 1.0f, float farPlane = 1000.0f)
{
    assert(fabs(farPlane - nearPlane) >= 0.01f);
    assert(fabs(sin(fov / 2)) >= 0.01f);

    float f = cosf(fov / 2.0f) / sinf(fov / 2.0f);

    mat = glm::mat4(0.0f);

    mat[0][0] = f / aspect;
    mat[1][1] = f;
    mat[2][2] = (nearPlane + farPlane) / (nearPlane - farPlane);
    mat[2][3] = -1.0f;
    mat[3][2] = (2.0f * farPlane * nearPlane) / (nearPlane - farPlane);
}

//! Loads an othogonal projection matrix
/**
 * \param mat         result matrix
 * \param left,right  coordinates for left and right vertical clipping planes
 * \param bottom,top  coordinates for bottom and top horizontal clipping planes
 * \param zNear,zFar  distance to nearer and farther depth clipping planes
 */
inline void LoadOrthoProjectionMatrix(glm::mat4 &mat, float left, float right, float bottom, float top,
                                      float zNear = -1.0f, float zFar = 1.0f)
{
    mat = glm::mat4(1.0f);

    mat[0][0] =  2.0f / (right - left);
    mat[1][1] =  2.0f / (top - bottom);
    mat[2][2] = -2.0f / (zFar - zNear);

    mat[3][0] =  - (right + left) / (right - left);
    mat[3][1] =  - (top + bottom) / (top - bottom);
    mat[3][2] =  - (zFar + zNear) / (zFar - zNear);
}

//! Loads a translation matrix from given vector
/**
 * \param mat   result matrix
 * \param trans vector of translation
 */
inline void LoadTranslationMatrix(glm::mat4 &mat, const glm::vec3 &trans)
{
    mat = glm::translate(glm::mat4(1.0f), trans);
}

//! Loads a scaling matrix fom given vector
/**
 * \param mat    result matrix
 * \param scale  vector with scaling factors for X, Y, Z
 */
inline void LoadScaleMatrix(glm::mat4 &mat, const glm::vec3 &scale)
{
    mat = glm::scale(glm::mat4(1.0f), scale);
}

//! Loads a rotation matrix along the X axis
/**
 * \param mat    result matrix
 * \param angle  angle [radians]
 */
inline void LoadRotationXMatrix(glm::mat4 &mat, float angle)
{
    mat = glm::mat4(1.0f);

    mat[1][1] =  cosf(angle);
    mat[1][2] =  sinf(angle);
    mat[2][1] = -sinf(angle);
    mat[2][2] =  cosf(angle);
}

//! Loads a rotation matrix along the Y axis
/**
 * \param mat    result matrix
 * \param angle  angle [radians]
 */
inline void LoadRotationYMatrix(glm::mat4 &mat, float angle)
{
    mat = glm::mat4(1.0f);

    mat[0][0] =  cosf(angle);
    mat[0][2] = -sinf(angle);
    mat[2][0] =  sinf(angle);
    mat[2][2] =  cosf(angle);
}

//! Loads a rotation matrix along the Z axis
/**
 * \param mat    result matrix
 * \param angle  angle [radians]
 */
inline void LoadRotationZMatrix(glm::mat4 &mat, float angle)
{
    mat = glm::mat4(1.0f);

    mat[0][0] =  cosf(angle);
    mat[0][1] =  sinf(angle);
    mat[1][0] = -sinf(angle);
    mat[1][1] =  cosf(angle);
}

//! Loads a rotation matrix along the given axis
/**
 * \param mat    result matrix
 * \param dir    axis of rotation
 * \param angle  angle [radians]
 */
inline void LoadRotationMatrix(glm::mat4 &mat, const glm::vec3 &dir, float angle)
{
    float cos = cosf(angle);
    float sin = sinf(angle);
    glm::vec3 v = glm::normalize(dir);

    mat = glm::mat4(1.0f);

    mat[0][0] = (v.x * v.x) * (1.0f - cos) + cos;
    mat[0][1] = (v.x * v.y) * (1.0f - cos) - (v.z * sin);
    mat[0][2] = (v.x * v.z) * (1.0f - cos) + (v.y * sin);

    mat[1][0] = (v.y * v.x) * (1.0f - cos) + (v.z * sin);
    mat[1][1] = (v.y * v.y) * (1.0f - cos) + cos ;
    mat[1][2] = (v.y * v.z) * (1.0f - cos) - (v.x * sin);

    mat[2][0] = (v.z * v.x) * (1.0f - cos) - (v.y * sin);
    mat[2][1] = (v.z * v.y) * (1.0f - cos) + (v.x * sin);
    mat[2][2] = (v.z * v.z) * (1.0f - cos) + cos;
}

//! Calculates the matrix to make three rotations in the order X, Z and Y
inline void LoadRotationXZYMatrix(glm::mat4 &mat, const glm::vec3 &angles)
{
    glm::mat4 temp;
    LoadRotationXMatrix(temp, angles.x);

    LoadRotationZMatrix(mat, angles.z);
    mat = temp * mat;

    LoadRotationYMatrix(temp, angles.y);
    mat = temp * mat;
}

//! Calculates the matrix to make three rotations in the order Z, X and Y
inline void LoadRotationZXYMatrix(glm::mat4 &mat, const glm::vec3 &angles)
{
    glm::mat4 temp;
    LoadRotationZMatrix(temp, angles.z);

    LoadRotationXMatrix(mat, angles.x);
    mat = temp * mat;

    LoadRotationYMatrix(temp, angles.y);
    mat = temp * mat;
}

//! Returns the distance between projections on XZ plane of two vectors
inline float DistanceProjected(const glm::vec3 &a, const glm::vec3 &b)
{
    return sqrtf( (a.x-b.x)*(a.x-b.x) +
                  (a.z-b.z)*(a.z-b.z) );
}

//! Returns the normal vector to a plane
/**
 * \param p1,p2,p3 points defining the plane
 */
inline glm::vec3 NormalToPlane(const glm::vec3 &p1, const glm::vec3 &p2, const glm::vec3 &p3)
{
    glm::vec3 u = p3 - p1;
    glm::vec3 v = p2 - p1;

    return glm::normalize(glm::cross(u, v));
}

//! Returns a point on the line \a p1 - \a p2, in \a dist distance from \a p1
/**
 * \param p1,p2  line start and end
 * \param dist   scaling factor from \a p1, relative to distance between \a p1 and \a p2
 */
inline glm::vec3 SegmentPoint(const glm::vec3 &p1, const glm::vec3 &p2, float dist)
{
    glm::vec3 direction = glm::normalize(p2 - p1);

    return p1 + direction * dist;
}

//! Returns the distance between given point and a plane
/**
 * \param p      the point
 * \param a,b,c  points defining the plane
 */
inline float DistanceToPlane(const glm::vec3 &a, const glm::vec3 &b,
                             const glm::vec3 &c, const glm::vec3 &p)
{
    glm::vec3 n = NormalToPlane(a, b, c);
    float d = -(n.x*a.x + n.y*a.y + n.z*a.z);

    return fabs(n.x*p.x + n.y*p.y + n.z*p.z + d);
}

//! Checks if two planes defined by three points are the same
/**
 * \param plane1  array of three vectors defining the first plane
 * \param plane2  array of three vectors defining the second plane
 */
inline bool IsSamePlane(const glm::vec3 (&plane1)[3], const glm::vec3 (&plane2)[3])
{
    glm::vec3 n1 = NormalToPlane(plane1[0], plane1[1], plane1[2]);
    glm::vec3 n2 = NormalToPlane(plane2[0], plane2[1], plane2[2]);

    if ( fabs(n1.x-n2.x) > 0.1f ||
         fabs(n1.y-n2.y) > 0.1f ||
         fabs(n1.z-n2.z) > 0.1f )
        return false;

    float dist = DistanceToPlane(plane1[0], plane1[1], plane1[2], plane2[0]);
    if ( dist > 0.1f )
        return false;

    return true;
}

//! Calculates the intersection "i" right "of" the plane "abc" (TODO: ?)
inline bool Intersect(const glm::vec3 &a, const glm::vec3 &b, const glm::vec3 &c,
                      const glm::vec3 &d, const glm::vec3 &e, glm::vec3 &i)
{
    float d1 = (d.x-a.x)*((b.y-a.y)*(c.z-a.z)-(c.y-a.y)*(b.z-a.z)) -
               (d.y-a.y)*((b.x-a.x)*(c.z-a.z)-(c.x-a.x)*(b.z-a.z)) +
               (d.z-a.z)*((b.x-a.x)*(c.y-a.y)-(c.x-a.x)*(b.y-a.y));

    float d2 = (d.x-e.x)*((b.y-a.y)*(c.z-a.z)-(c.y-a.y)*(b.z-a.z)) -
               (d.y-e.y)*((b.x-a.x)*(c.z-a.z)-(c.x-a.x)*(b.z-a.z)) +
               (d.z-e.z)*((b.x-a.x)*(c.y-a.y)-(c.x-a.x)*(b.y-a.y));

    if (d2 == 0)
        return false;

    i.x = d.x + d1/d2*(e.x-d.x);
    i.y = d.y + d1/d2*(e.y-d.y);
    i.z = d.z + d1/d2*(e.z-d.z);

    return true;
}

//! Calculates the intersection of the straight line passing through p (x, z)
/** Line is parallel to the y axis, with the plane abc. Returns p.y. (TODO: ?) */
inline bool IntersectY(const glm::vec3 &a, const glm::vec3 &b, const glm::vec3 &c, glm::vec3 &p)
{
    float d  = (b.x-a.x)*(c.z-a.z) - (c.x-a.x)*(b.z-a.z);
    float d1 = (p.x-a.x)*(c.z-a.z) - (c.x-a.x)*(p.z-a.z);
    float d2 = (b.x-a.x)*(p.z-a.z) - (p.x-a.x)*(b.z-a.z);

    if (d == 0.0f)
        return false;

    p.y = a.y + d1/d*(b.y-a.y) + d2/d*(c.y-a.y);

    return true;
}

//! Calculates the end point
inline glm::vec3 LookatPoint(const glm::vec3 &eye, float angleH, float angleV, float length)
{
    glm::vec3 lookat = eye;
    lookat.z += length;

    RotatePoint(eye, angleH, angleV, lookat);

    return lookat;
}

//! Transforms the point \a p by matrix \a m
/** Is equal to multiplying the matrix by the vector (of course without perspective divide). */
inline glm::vec3 Transform(const glm::mat4 &m, const glm::vec3 &p)
{
    return glm::vec3(m * glm::vec4(p, 1.0f));
}


//! Transforms a vector \v by a matrix \m with perspective divide
inline glm::vec3 TransformDivide(const glm::mat4& m, const glm::vec3& v)
{
    glm::vec4 result = m * glm::vec4(v, 1.0f);

    if (IsZero(result.w))
        return glm::vec3(result);

    return glm::vec3(result) / result.w;
}

//! Calculates the projection of the point \a p on a straight line \a a to \a b
/**
 * \param p      point to project
 * \param a,b    two ends of the line
 */
inline glm::vec3 Projection(const glm::vec3 &a, const glm::vec3 &b, const glm::vec3 &p)
{
    float k = glm::dot(b - a, p - a);
    k /= glm::dot(b - a, b - a);

    return a + k*(b-a);
}

//! Calculates point of view to look at a center two angles and a distance
inline glm::vec3 RotateView(glm::vec3 center, float angleH, float angleV, float dist)
{
    glm::mat4 mat1, mat2;
    LoadRotationZMatrix(mat1, -angleV);
    LoadRotationYMatrix(mat2, -angleH);

    glm::mat4 mat = mat2 * mat1;

    glm::vec3 eye{};
    eye.x = 0.0f+dist;
    eye.y = 0.0f;
    eye.z = 0.0f;
    eye = Transform(mat, eye);

    return eye+center;
}


} // namespace Math