////////////////////////////////////////////////////////////////////////////////
//
// Paella
// Copyright (C) 2015 - Thomas FORGIONE, Emilie JALRAS, Marion LENFANT, Thierry MALON, Amandine PAILLOUX
// Authors :
//     Thomas FORGIONE
//     Emilie JALRAS
//     Marion LENFANT
//     Thierry MALON
//     Amandine PAILLOUX
//
// This file is part of the project Paella
// This software is provided 'as-is', without any express or implied warranty.
// In no event will the authors be held liable for any damages arising from the use of this software.
//
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it freely,
// subject to the following restrictions:
//
// 1. The origin of this software must not be misrepresented;
//    you must not claim that you wrote the original software.
//    If you use this software in a product, an acknowledgment
//    in the product documentation would be appreciated but is not required.
//
// 2. Altered source versions must be plainly marked as such,
//    and must not be misrepresented as being the original software.
//
// 3. This notice may not be removed or altered from any source distribution.
////////////////////////////////////////////////////////////////////////////////
#include <iostream>
#include <fstream>
#include <sstream>
#include <cmath>
#include "Geometry/Vector.hpp"
#include "Geometry/Base.hpp"
#include "Geometry/Spline.hpp"

namespace geo
{

template<typename... Types>
Spline<Types...>::Spline() : controlPoints{}, nodes{}, degree{}
{

}

template<typename... Types>
std::vector<Circle<float>> Spline<Types...>::computeCircles(unsigned int nbCircles, unsigned int const nbPoints, unsigned int const globalOffset) const
{
    std::vector<Circle<float>> circles;
    unsigned int offset = 0;

    Vector3<float> v{0.0f,0.0f,1.0f};

    for (unsigned int i = 0; i < nbCircles; i++)
    {
        auto circle = computeCircle(static_cast<float>(i)/(nbCircles-1), nbPoints, v, globalOffset+offset);
        if (!circle.points.empty())
        {
            circles.push_back(circle);
            offset += nbPoints;
        }
    }

    return circles;

}

template<typename... Types>
Circle<float> Spline<Types...>::computeCircle(float t, unsigned int const nbPoints, Vector3<float>& v, unsigned int const offset) const
{
    Circle<float> circlePoints;

    auto S_t = (*this)(t);
    auto S_prime_t = prime(t);
    auto C_t = std::get<0>(S_t);
    auto C_prime_t = std::get<0>(S_prime_t);
    auto r_t = std::get<1>(S_t);
    auto r_prime_t = std::get<1>(S_prime_t);

    auto a = C_prime_t.x();
    auto b = C_prime_t.y();
    auto c = C_prime_t.z();
    auto d = r_t * r_prime_t - C_t.x() * C_prime_t.x() - C_t.y() * C_prime_t.y() - C_t.z() * C_prime_t.z();

    auto dist = std::abs(a * C_t.x() + b * C_t.y() + c * C_t.z() + d)/std::sqrt(a*a+b*b+c*c);

    C_prime_t /= C_prime_t.norm();
    auto C_P_t = C_t;
    auto C_P_t1 = C_t + dist * C_prime_t;
    auto C_P_t2 = C_t - dist * C_prime_t;

    float dist1 = std::abs(a * C_P_t1.x() + b * C_P_t1.y() + c * C_P_t1.z() + d);
    float dist2 = std::abs(a * C_P_t2.x() + b * C_P_t2.y() + c * C_P_t2.z() + d);

    if (dist1 < dist2)
        C_P_t = C_P_t1;
    else
        C_P_t = C_P_t2;

    auto C_t_C_P_t = C_P_t - C_t;

    auto r_P_t = r_t*r_t - C_t_C_P_t.norm2();

    if (r_P_t < 0)
        return {};
    else
        r_P_t = std::sqrt(r_P_t);

    circlePoints.center = C_P_t;
    v /= v.norm();

    constexpr auto baseVector1 = Vector3<float>{1.0f,0.0f,0.0f};
    auto baseVector2 = crossProduct(C_prime_t,baseVector1);
    baseVector2 /= baseVector2.norm();

    auto proj = (v*baseVector1)*baseVector1 + (v*baseVector2)*baseVector2;
    proj /= proj.norm();
    v = proj;

    auto u = crossProduct(C_prime_t, v);
    u /= u.norm();

    for (unsigned int i = 0 ; i < nbPoints ; i++)
    {
        auto theta = 2 * M_PI * i /nbPoints ;
        auto w = std::cos(theta) * v + std::sin(theta) * u;
        auto circlePoint = C_P_t + r_P_t * w;

        circlePoints.points.push_back(std::make_pair(i+offset,circlePoint));
    }

    return circlePoints;
}

template<typename... Types>
std::tuple<Types...> Spline<Types...>::operator()(float f) const
{
    return detail::evalSpline(controlPoints, nodes, degree, f);
}

template<typename... Types>
std::tuple<Types...> Spline<Types...>::prime(float f) const
{
    return detail::evalDerivativeSpline(controlPoints, nodes, degree, f);
}

template<typename... Types>
std::ostream& operator<<(std::ostream& out, Spline<Types...> const& spline)
{
    out << "d " << spline.degree << "\n";
    out << "n ";

    for (auto const& node : spline.nodes)
        out << node << " ";
    out << "\n";

    using namespace detail;

    for (auto const& tuple : spline.controlPoints)
    {
        out << "p " << tuple << "\n";
    }

    return out;
}


template<typename... Types>
void Spline<Types...>::addControlPoint(std::string const& point)
{
    std::tuple<Types...> tuple;
    std::stringstream stream{point};
    pae::detail::loadFromStream(tuple, stream);
    controlPoints.push_back(tuple);
}

namespace detail
{

template<std::size_t I, std::size_t J, typename... Types>
struct evalSplineHelper
{
    std::tuple<Types...> operator()(
        std::vector<std::tuple<Types...>> const& controlPoints,
        std::vector<float> const& nodes,
        int degree,
        float f,
        std::tuple<Types...>& tuple)
    {
        std::get<J>(tuple) = 0;

        for (auto i = 0u; i < nodes.size() - degree - 1; i++)
        {
            std::get<J>(tuple) += base(i, degree, f, nodes) * std::get<J>(controlPoints[i]);
        }

        evalSplineHelper<I-1,J+1,Types...>{}(controlPoints, nodes, degree, f, tuple);

        return tuple;
    }
};

template<std::size_t J, typename... Types>
struct evalSplineHelper<0,J,Types...>
{
    std::tuple<Types...> operator()(
        std::vector<std::tuple<Types...>> const& controlPoints,
        std::vector<float> const& nodes,
        int degree,
        float f,
        std::tuple<Types...>& tuple)
    {
        // Nothing to do here
        return tuple;
    }

};

template<typename... Types>
std::tuple<Types...> evalSpline(std::vector<std::tuple<Types...>> const& controlPoints , std::vector<float> const& nodes, int degree, float f)
{
    std::tuple<Types...> tuple;

    return evalSplineHelper<std::tuple_size<std::tuple<Types...>>::value, 0, Types...>{}(
                controlPoints, nodes, degree, f, tuple);

    return tuple;
}

template<std::size_t I, std::size_t J, typename... Types>
struct evalDerivativeSplineHelper
{

    std::tuple<Types...> operator()(std::vector<std::tuple<Types...>> const& controlPoints , std::vector<float> const& nodes, int degree, float f, std::tuple<Types...>& tuple)
    {
        std::get<J>(tuple) = 0;

        for (auto i = 0u; i < nodes.size() - degree -1 ; i++)
        {
            float denom1 = nodes[i+degree]-nodes[i];
            float denom2 = nodes[i+degree+1]-nodes[i+1];

            if (std::abs(denom1) > 0.001)
                std::get<J>(tuple) += (degree/denom1)*base(i,degree-1,f,nodes)*std::get<J>(controlPoints[i]);

            if (std::abs(denom2) > 0.001)
                std::get<J>(tuple) -= (degree/denom2)*base(i+1,degree-1,f,nodes)*std::get<J>(controlPoints[i]);
        }

        // Appel recursif template
        evalDerivativeSplineHelper<I-1,J+1,Types...>{}(controlPoints, nodes, degree, f, tuple);

        return tuple;
    }
};

template<std::size_t J,typename... Types>
struct evalDerivativeSplineHelper<0,J,Types...>
{
    std::tuple<Types...> operator()(std::vector<std::tuple<Types...>> const& controlPoints , std::vector<float> const& nodes, int degree, float f, std::tuple<Types...>& tuple)
    {
        // Nothing to do here
        return tuple;
    }

};

template<typename... Types>
std::tuple<Types...> evalDerivativeSpline(std::vector<std::tuple<Types...>> const& controlPoints, std::vector<float> const& nodes, int degree, float f)
{
    std::tuple<Types...> tuple;

    return evalDerivativeSplineHelper<std::tuple_size<std::tuple<Types...>>::value,0,Types...>{}(controlPoints, nodes, degree, f, tuple);

}

} // namespace detail

} // namespace geo