/****************************************************************************
* Copyright (C) 2009      Daniel Chappuis                                  *
****************************************************************************
* This file is part of ReactPhysics3D.                                     *
*                                                                          *
* ReactPhysics3D is free software: you can redistribute it and/or modify   *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or     *
* (at your option) any later version.                                      *
*                                                                          *
* ReactPhysics3D 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 Lesser General Public License for more details.                      *
*                                                                          *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>.   *
***************************************************************************/

// TODO : Mathematics library : Check everywhere that in member methods we use attributes access instead of getter and setter.

#ifndef MATHEMATICS_H
#define MATHEMATICS_H

// Libraries
#include "Matrix.h"
#include "Matrix3x3.h"
#include "Polygon3D.h"
#include "Quaternion.h"
#include "Segment3D.h"
#include "Vector.h"
#include "Vector3D.h"
#include "constants.h"
#include "exceptions.h"
#include "mathematics_functions.h"
#include <cstdio>
#include <cassert>
#include <cmath>

// ---------- Mathematics functions ---------- //

// TODO : Test this method
// Rotate a vector according to a rotation quaternion.
// The function returns the vector rotated according to the quaternion in argument
inline reactphysics3d::Vector3D rotateVectorWithQuaternion(const reactphysics3d::Vector3D& vector, const reactphysics3d::Quaternion& quaternion) {
    // Convert the vector into a quaternion
    reactphysics3d::Quaternion vectorQuaternion(0, vector);

    // Compute the quaternion rotation result
    reactphysics3d::Quaternion quaternionResult = (quaternion * vectorQuaternion) * quaternion.getInverse();

    // Convert the result quaternion into a vector
    return quaternionResult.vectorV();
}

// TODO : Test this method
// Given two lines (given by the points "point1", "point2" and the vectors "d1" and "d2" that are not parallel, this method returns the values
// "alpha" and "beta" such that the two points P1 and P2 are the two closest point between the two lines and such that
// P1 = point1 + alpha * d1
// P2 = point2 + beta * d2
inline void closestPointsBetweenTwoLines(const reactphysics3d::Vector3D& point1, const reactphysics3d::Vector3D& d1, const reactphysics3d::Vector3D& point2,
                                         const reactphysics3d::Vector3D& d2, double* alpha, double* beta) {

    reactphysics3d::Vector3D r = point1 - point2;
    double a = d1.scalarProduct(d1);
    double b = d1.scalarProduct(d2);
    double c = d1.scalarProduct(r);
    double e = d2.scalarProduct(d2);
    double f = d2.scalarProduct(r);
    double d = a*e-b*b;

    // The two lines must not be parallel
    assert(!reactphysics3d::approxEqual(d, 0.0));

    // Compute the "alpha" and "beta" values
    *alpha = (b*f -c*e)/d;
    *beta = (a*f-b*c)/d;
}

/*
// TODO : Test this method
// Move a set of points by a given vector.
// The method returns a set of points moved by the given vector.
inline std::vector<reactphysics3d::Vector3D> movePoints(const std::vector<reactphysics3d::Vector3D>& points, const reactphysics3d::Vector3D& vector) {
    std::vector<reactphysics3d::Vector3D> result;

    // For each point of the set
    for (unsigned int i=0; i<points.size(); ++i) {
        // Move the point
        result.push_back(points.at(i) + vector);
    }

    // Return the result set of points
    return result;
}

// TODO : Test this method
// Return the intersection between two Segment3D that are on the same line. The result of the intersection
// will be another Segment3D.
inline reactphysics3d::Segment3D computeParallelSegmentsIntersection(const reactphysics3d::Segment3D& segment1, const reactphysics3d::Segment3D& segment2) {
    // The two segments should be colinear
    assert(approxEqual(std::abs(segment1.getSegmentVector().scalarProduct(segment2.getSegmentVector())), segment1.getSegmentVector().length() * segment2.getSegmentVector().length()));

    // Result segment
    reactphysics3d::Segment3D resultSegment;

    // Compute the vector of the line where both segments are
    reactphysics3d::Vector3D lineVector = segment1.getSegmentVector();

    // Compute the projection of the two points of the second segment on the line
    double projSegment2PointA = lineVector.getUnit().scalarProduct(segment2.getPointA() - segment1.getPointA());
    double projSegment2PointB = lineVector.getUnit().scalarProduct(segment2.getPointB() - segment1.getPointA());

    std::cout << "Segment 2 - Proj A : " << projSegment2PointA << std::endl;
    std::cout << "Segment 2 - Proj B : " << projSegment2PointB << std::endl;
    std::cout << "Segment 1 - length : " << segment1.getLength() << std::endl;

    // The projections intervals should intersect
    assert(!(projSegment2PointA < 0.0 && projSegment2PointB < 0.0));
    assert(!(projSegment2PointA > segment1.getLength() && projSegment2PointB > segment1.getLength()));

    // Return the segment intersection according to the configuration of two projection intervals
    if (projSegment2PointA >= 0 && projSegment2PointA <= segment1.getLength() && projSegment2PointB >= segment1.getLength()) {
        resultSegment.setPointA(segment2.getPointA());
        resultSegment.setPointB(segment1.getPointB());
        return resultSegment;
    }
    else if (projSegment2PointA <= 0 && projSegment2PointB >= 0 && projSegment2PointB <= segment1.getLength()) {
        resultSegment.setPointA(segment1.getPointA());
        resultSegment.setPointB(segment2.getPointB());
        return resultSegment;
    }
    else if (projSegment2PointA <= 0 && projSegment2PointB >= segment1.getLength()) {
        return segment1;
    }
    else if (projSegment2PointA <= segment1.getLength() && projSegment2PointB <= segment1.getLength()) {
        return segment2;
    }

    // We should never go here
    assert(false);
}
*/

#endif