/***************************************************************************
 *   Copyright (c) 2007                                                    *
 *   Joachim Zettler <Joachim.Zettler@gmx.de>                              *                                  *
 *   Mohamad Najib Muhammad Noor <najib_bean@yahoo.co.uk>                  *
 *                                                                         *
 *   This file is part of the FreeCAD CAx development system.              *
 *                                                                         *
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Library General Public           *
 *   License as published by the Free Software Foundation; either          *
 *   version 2 of the License, or (at your option) any later version.      *
 *                                                                         *
 *   This library  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 Library General Public License for more details.                  *
 *                                                                         *
 *   You should have received a copy of the GNU Library General Public     *
 *   License along with this library; see the file COPYING.LIB. If not,    *
 *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
 *   Suite 330, Boston, MA  02111-1307, USA                                *
 *                                                                         *
 ***************************************************************************/

/*********ROUTINE.CPP*********/
#include "PreCompiled.h"
#include <cmath>
#include "routine.h"


///*********BINDINGS********/
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/atlas/clapack.hpp>

using namespace boost::numeric::bindings;
using namespace boost::numeric;



/*! \brief Numerical Integration according to trapezoid rules

 This routine assumes that the Intergral values are already corresponding with WithRespectTo
 i.e: Intergral[i] == Intergral(WithRespectTo[i]);
 Also, it is assumed that WithRespectTo are evenly spaced with it's stepWidth
*/
double Routines::TrapezoidIntergration(const std::vector<double> &WithRespectTo, const std::vector<double> &Integral)
{
    m_h = 0;
    m_result = 0;
    m_N = Integral.size();
    if (m_N==1)  // trapzf(x,y) returns zero for a scalar x
    {
        m_result = 0;
        return m_result;
    }

    m_h = WithRespectTo[1] - WithRespectTo[0]; /* the integration stepsize */
    for (int i=1; i< m_N - 1; i++)
        m_result = m_result + Integral[i];

    m_result = m_h/2 * (Integral[0] + 2*(m_result) + Integral[m_N -1]);
    return m_result;
}

std::vector<double> Routines::NewtonStep(std::vector<double> &F,std::vector<std::vector<double> > &DF)
{
    // löst folgendes Gleichungssystem: DF*x = F
    int siz = (int) F.size();
    std::vector<double> x_new(siz);
    std::vector<int> piv(siz);            // pivotelement

    ublas::matrix<double> A(siz, siz);
    ublas::matrix<double> b(1, siz);

    // füllt blas-matrizen
    for (unsigned int i=0; i<siz; ++i)
    {
        b(0,i) = -F[i];
        for (unsigned int j=0; j<siz; ++j)
        {
            A(i,j) = DF[i][j];
        }
    }

    /*cout << b(0,0) << "," << b(0,1) << "," << b(0,2) << endl;
    cout << A(0,0) << "," << A(0,1) << "," << A(0,2) << endl;
    cout << A(1,0) << "," << A(1,1) << "," << A(1,2) << endl;
    cout << A(2,0) << "," << A(2,1) << "," << A(2,2) << endl;*/


    atlas::lu_factor(A,piv);              // führt LU-Zerlegung durch
    atlas::getrs(A,piv,b);                // löst Gl.system A*x = b (b wird mit der Lösung überschrieben)

    for (unsigned int i=0; i<siz; ++i)
    {
        x_new[i] = b(0,i);
    }

    return x_new;
}


/*! \brief Cramer-Rule Solver

 Cramer-Rule solver for 2x2 Matrix.

 RHS1 * LHS = RHS2, LHS will be computed
*/
void Routines::CramerSolve(std::vector< std::vector<double> > &RHS1, std::vector<double>& RHS2, std::vector<double> &LHS)
{
    double MainMatrixDet = det2(RHS1);
    std::vector< std::vector<double> > Temp(2,std::vector<double>(2,0.0));
    for (int i = 0; i < 2; i++)
        for (int j = 0; j < 2; j++)
            Temp[i][j] = RHS1[i][j];

    //first pass
    for (int i = 0; i < 2; i++)
        Temp[i][0] = RHS2[i];

    LHS[0] = det2(Temp) / MainMatrixDet;

    //restore
    for (int i = 0; i < 2; i++)
        for (int j = 0; j < 2; j++)
            Temp[i][j] = RHS1[i][j];

    //second pass
    for (int i = 0; i < 2; i++)
        Temp[i][1] = RHS2[i];

    LHS[1] = det2(Temp) / MainMatrixDet;
}

/*! \brief Calculate angle between two vectors

 Dependencies: Vector3D definitions and routines
*/
double Routines::CalcAngle(Base::Vector3f a,Base::Vector3f  b,Base::Vector3f c)
{
    Base::Vector3f First = a - b;
    Base::Vector3f Second = c - b;
    Base::Vector3f Third = c - a;
    //double test1 = First.Length(), test2 = Second.Length(), test3 = Third.Length();
    double UpperTerm = (First.Length() * First.Length()) + (Second.Length() *Second.Length()) - (Third.Length() * Third.Length() );
    double LowerTerm = 2 * First.Length() * Second.Length() ;
    double ang = acos( UpperTerm / LowerTerm );
    return ang;
}





/*! \brief Algorithm A2.1 from NURBS Book Page 68*/
int Routines::FindSpan(int n, int p, double u, std::vector<double> KnotSequence)
{
    if (u == KnotSequence[n+1])
        return n;
    int low, high, mid, i;
    i = 0;
    low = p;
    high = n+1;
    mid = (low+high) / 2;
    while (u < KnotSequence[mid] || u >= KnotSequence[mid+1])
    {
        if (u < KnotSequence[mid])
            high = mid;
        else
            low = mid;
        mid = (low + high) / 2;
        i++;
    }

    return mid;

}


/*! \brief Algorithm A2.4 from NURBS Book page 70*/
void Routines::Basisfun(int i, double u, int p, std::vector<double> &KnotSequence, std::vector<double> &output)
{
    double temp, saved;
    std::vector<double> leftie(p+1, 0.0);
    std::vector<double> rightie(p+1, 0.0);
    output[0] = 1.0;
    for (int j = 1; j <= p; j++)
    {
        leftie[j] = u - KnotSequence[i+1-j];
        rightie[j] = KnotSequence[i+j] - u;
        saved = 0.0;
        for (int r = 0; r < j; r++)
        {
            temp = output[r] / (rightie[r+1] + leftie[j-r]);
            output[r] = saved + (rightie[r+1]*temp);
            saved = leftie[j-r]*temp;
        }
        output[j] = saved;
    }
}
/*! \brief Algorithm A2.3 from NURBS Book Page 72 */
void Routines::DersBasisFuns(int i, double u, int p, int n,
                             std::vector<double> &KnotSequence, std::vector< std::vector<double> > &Derivate)
{
    std::vector< std::vector<double> > ndu(p+1, std::vector<double>(p+1,0.0));
    std::vector< std::vector<double> > a(2, std::vector<double>(p+1,0.0));
    std::vector<double> leftie(p+1, 0.0);
    std::vector<double> rightie(p+1, 0.0);
    ndu[0][0] = 1.0;
    for (int j = 1; j <= p; j++)
    {
        leftie[j] = u - KnotSequence[i+1-j];
        rightie[j] = KnotSequence[i+j] - u;
        double saved = 0.0;
        for (int r = 0; r < j; r++)
        {
            ndu[j][r] = rightie[r+1] + leftie[j-r];
            double temp = ndu[r][j-1] / ndu[j][r];

            ndu[r][j] = saved + rightie[r+1]*temp;
            saved = leftie[j-r]*temp;
        }
        ndu[j][j] = saved;
    }
    for (int j = 0; j <= p; j++)
        Derivate[0][j] = ndu[j][p];

    for (int r = 0; r <= p; r++)
    {
        int j1, j2;
        int s1 = 0;
        int s2 = 1;
        a[0][0] = 1.0;
        for (int k = 1; k <= n; k++)
        {
            double d = 0.0;
            int j;
            int rk = r - k;
            int pk = p - k;
            if (r >= k)
            {
                a[s2][0] = a[s1][0] / ndu[pk+1][rk];
                d += a[s2][0] * ndu[rk][pk];
            }

            if (rk >= -1)
                j1 = 1;
            else j1 = -rk;

            if (r -1 <= pk)
                j2 = k -1;
            else j2 = p - r;

            for (j = j1; j <= j2; j++)
            {
                a[s2][j] = (a[s1][j]-a[s1][j-1]) / ndu[pk+1][rk+j];
                d += a[s2][j] *  ndu[rk+j][pk];
            }

            if (r <= pk)
            {
                a[s2][k] = -a[s1][k-1] / ndu[pk+1][r];
                d += a[s2][k] * ndu[r][pk];
            }

            Derivate[k][r] = d;
            j = s1;
            s1 = s2;
            s2 = j;


        }
    }
    int r = p;
    for (int k = 1; k <= n; k++)
    {
        for (int j = 0; j <= p; j++)
            Derivate[k][j] *= r;

        r*= (p-k);
    }


}
/*! \brief Translation from nrbeval.m from NURBS Toolbox. WARNING: ONLY POINT EVALUATION ARE CONSIDERED!!!! */
void Routines::PointNrbEval(std::vector<double> &p, std::vector<double> &CurPoint,  NURBS &MainNurb)
{
    if (!p.empty())
        p.clear();
    int i = 0, j = 0;
    //val = reshape(nurbs.coefs,4*num1,num2);
    std::vector< std::vector<double> > CntrlPoints(((MainNurb.MaxU+1)*3), std::vector<double>((MainNurb.MaxV+1), 0.0));
    for (unsigned int a = 0; a < MainNurb.CntrlArray.size(); )
    {
        CntrlPoints[i][j] = MainNurb.CntrlArray[a];
        CntrlPoints[i+1][j] = MainNurb.CntrlArray[a+1];
        CntrlPoints[i+2][j] = MainNurb.CntrlArray[a+2];
        i += 3;
        a += 3;
        if (i == ((MainNurb.MaxU+1)*3))
        {
            i = 0;
            j++;
        }

    }
    //v direction...?
    i = 0;
    std::vector<double> Output(CntrlPoints.size(), 0.0);
    PointBSPEval(MainNurb.DegreeV, CntrlPoints, MainNurb.KnotV, Output, CurPoint[1]);
    std::vector< std::vector<double> > RedoneOutput(3, std::vector<double>(MainNurb.MaxU+1, 0.0));
    for (unsigned int a = 0; a < Output.size(); )
    {
        RedoneOutput[0][i] = Output[a];
        RedoneOutput[1][i] = Output[a+1];
        RedoneOutput[2][i] = Output[a+2];
        a += 3;
        i++;
    }
    std::vector<double> OutVect(4,0.0);
    PointBSPEval(MainNurb.DegreeU, RedoneOutput, MainNurb.KnotU, OutVect,CurPoint[0]);
    p.push_back(OutVect[0]);
    p.push_back(OutVect[1]);
    p.push_back(OutVect[2]);


}

/*! \brief Translation from bspeval.m from NURBS Toolbox. WARNING: ONLY POINT EVALUATION ARE CONSIDERED!!!! */
void Routines::PointBSPEval(int Degree, std::vector< std::vector<double> > &Cntrl, std::vector<double> &KnotSequence,
                            std::vector< double > &Output, double CurEvalPoint)
{
    std::vector<double> Basis(Degree+1, 0.0);
    int s = FindSpan(Cntrl[0].size() - 1,Degree,CurEvalPoint,KnotSequence);
    Basisfun(s,CurEvalPoint,Degree,KnotSequence,Basis);
    int tmp1 = s - Degree;
    for (unsigned int row = 0;row < Cntrl.size();row++)
    {
        double tmp2 = 0.0;
        for (int i = 0; i <= Degree; i++)
            tmp2 += Basis[i] *Cntrl[row][tmp1+i];
        Output[row] = tmp2;
    }

}
/*! \brief Translation from nrbderivate.m from NURBS Toolbox. WARNING: ONLY POINT EVALUATION ARE CONSIDERED!!!! */
void Routines::PointNrbDerivate(NURBS MainNurb, std::vector<NURBS> &OutNurbs)
{
    if (!OutNurbs.empty())
        OutNurbs.clear();
    NURBS Temp;
    int i = 0, j = 0, k = 0;
    std::vector< std::vector<double> > ControlDerivate;
    std::vector<double> KnotDerivate;
    std::vector< std::vector<double> > CntrlPoints(((MainNurb.MaxV+1)*3), std::vector<double>((MainNurb.MaxU+1), 0.0));
    for (unsigned int a = 0; a < MainNurb.CntrlArray.size(); )
    {
        CntrlPoints[i][j] = MainNurb.CntrlArray[a];
        CntrlPoints[i+1][j] = MainNurb.CntrlArray[a+1];
        CntrlPoints[i+2][j] = MainNurb.CntrlArray[a+2];
        j++;
        a += 3;
        i = k*3;
        if (j == (MainNurb.MaxU+1))
        {
            j = 0;
            k++;
            i = k*3;
        }

    }

    //U derivate
    bspderiv(MainNurb.DegreeU, CntrlPoints, MainNurb.KnotU, ControlDerivate, KnotDerivate);
    Temp.CntrlArray.resize(ControlDerivate.size() * ControlDerivate[0].size());
    i = 0, j = 0, k = 0;
    for (unsigned int a = 0; a < Temp.CntrlArray.size(); )
    {
        Temp.CntrlArray[a] = ControlDerivate[i][j];
        Temp.CntrlArray[a+1] = ControlDerivate[i+1][j];
        Temp.CntrlArray[a+2] = ControlDerivate[i+2][j];
        j++;
        a += 3;
        i = k*3;
        if (j == (int)ControlDerivate[i].size())
        {
            j = 0;
            k++;
            i = k*3;
        }
    }
    Temp.KnotU = KnotDerivate;
    Temp.KnotV = MainNurb.KnotV;
    Temp.MaxKnotU = Temp.KnotU.size();
    Temp.MaxKnotV = Temp.KnotV.size();
    Temp.MaxU = ControlDerivate[0].size();
    Temp.MaxV = ControlDerivate.size() / 3;
    Temp.DegreeU = Temp.MaxKnotU - Temp.MaxU - 1;
    Temp.DegreeV = Temp.MaxKnotV - Temp.MaxV - 1;
    Temp.MaxU--;
    Temp.MaxV--;
    OutNurbs.push_back(Temp);
    //reset
    i = 0, j = 0, k = 0;
    CntrlPoints.clear();
    ControlDerivate.clear();
    KnotDerivate.clear();
    CntrlPoints.resize((MainNurb.MaxU+1)*3);
    for (unsigned int a = 0; a < CntrlPoints.size(); a++)
        CntrlPoints[a].resize(MainNurb.MaxV+1);

    for (unsigned int a = 0; a < MainNurb.CntrlArray.size(); )
    {
        CntrlPoints[i][j] = MainNurb.CntrlArray[a];
        CntrlPoints[i+1][j] = MainNurb.CntrlArray[a+1];
        CntrlPoints[i+2][j] = MainNurb.CntrlArray[a+2];
        i += 3;
        a += 3;
        if (i == ((MainNurb.MaxU+1)*3))
        {
            i = 0;
            j++;
        }

    }
    //V derivate
    bspderiv(MainNurb.DegreeV, CntrlPoints, MainNurb.KnotV, ControlDerivate, KnotDerivate);
    Temp.CntrlArray.resize(ControlDerivate.size() * ControlDerivate[0].size());
    i = 0, j = 0, k = 0;
    for (unsigned int a = 0; a < Temp.CntrlArray.size(); )
    {
        Temp.CntrlArray[a] = ControlDerivate[i][j];
        Temp.CntrlArray[a+1] = ControlDerivate[i+1][j];
        Temp.CntrlArray[a+2] = ControlDerivate[i+2][j];
        a += 3;
        i += 3;
        if (i == (int)ControlDerivate.size())
        {
            j++;
            i = 0;
        }
    }
    Temp.KnotU = MainNurb.KnotU;
    Temp.KnotV = KnotDerivate;
    Temp.MaxKnotU = Temp.KnotU.size();
    Temp.MaxKnotV = Temp.KnotV.size();
    Temp.MaxU = ControlDerivate.size() / 3;
    Temp.MaxV = ControlDerivate[0].size();
    Temp.DegreeU = Temp.MaxKnotU - Temp.MaxU - 1;
    Temp.DegreeV = Temp.MaxKnotV - Temp.MaxV - 1;
    Temp.MaxU--;
    Temp.MaxV--;
    OutNurbs.push_back(Temp);
}

/*! \brief Translation from bspderiv.m from NURBS Toolbox. WARNING: ONLY POINT EVALUATION ARE CONSIDERED!!!! */
void Routines::bspderiv(int d, std::vector< std::vector<double> > &c, std::vector<double> &k,
                        std::vector< std::vector<double> > &OutputC, std::vector<double> &Outputk)
{
    OutputC.resize(c.size());
    for (unsigned int i = 0; i < OutputC.size();  i++)
        OutputC[i].resize(c[i].size()-1);

    double tmp = 0.0;

    for (unsigned int i = 0; i < OutputC[0].size(); i++)
    {
        tmp = d / (k[i+d+1]-k[i+1]);
        for (unsigned int j = 0; j < OutputC.size(); j++)
            OutputC[j][i] = tmp * (c[j][i+1] -  c[j][i]);
    }
    Outputk.resize(k.size() - 2);
    for (unsigned int i = 0; i < Outputk.size(); i++)
        Outputk[i] = k[i+1];
}

/*! \brief Translation from nrbdeval.m from NURBS Toolbox. WARNING: ONLY POINT EVALUATION ARE CONSIDERED!!!! */
void Routines::NrbDEval(NURBS &MainNurb, std::vector<NURBS> &DerivNurb, std::vector<double> &Point,
                        std::vector<double> &EvalPoint, std::vector<std::vector<double> > &jac)
{
    if (!EvalPoint.empty())
        EvalPoint.clear();
    if (!jac.empty())
        jac.clear();
    std::vector<double> TempoPointDeriv;
    PointNrbEval(EvalPoint, Point, MainNurb);
    for (unsigned int i = 0; i < DerivNurb.size(); i++)
    {
        PointNrbEval(TempoPointDeriv, Point, DerivNurb[i]);
        jac.push_back(TempoPointDeriv);
        TempoPointDeriv.clear();
    }
}

/*! \brief Uniform Knot Generator */
void Routines::GenerateUniformKnot(int MaxCntrl, int NurbDegree, std::vector<double> &KnotSequence)
{
    if (!KnotSequence.empty())
        KnotSequence.clear();

    for (int i = 0; i < (MaxCntrl + NurbDegree + 2); i++)
        KnotSequence.push_back(0);

    for (int i = NurbDegree; i < MaxCntrl; i++)
        KnotSequence[i+1] = (double)(i - NurbDegree + 1.0) / (double)(MaxCntrl - NurbDegree + 1.0);

    for (unsigned int i = MaxCntrl+1; i < KnotSequence.size(); i++)
        KnotSequence[i] = 1;

}

/*! \brief Find the corner points

 Idea: From the neighbour list, find the shortest distance of a point to the given parameter X and Y.
 Parameter X and Y are the bounding boxes parameter of the mesh (combinations of Min and Max of X and Y)
*/
unsigned long Routines::FindCorner(float ParamX, float ParamY, std::vector<unsigned long> &v_neighbour, std::vector <Base::Vector3f> &v_pnts)
{
    unsigned int j = 0;
    bool change = false;
    //double distance = sqrt(((ParamX - ParameterMesh.GetPoint(v_neighbour[0])[0])*(ParamX - ParameterMesh.GetPoint(v_neighbour[0])[0])) +
    // ((ParamY - ParameterMesh.GetPoint(v_neighbour[0])[1])*(ParamY - ParameterMesh.GetPoint(v_neighbour[0])[1])));
    double distance = sqrt(((ParamX - v_pnts[0][0])*(ParamX -  v_pnts[0][0])) +
                           ((ParamY -  v_pnts[0][1])*(ParamY -  v_pnts[0][1])));
    for (unsigned int k = 1; k < v_neighbour.size(); ++k)
    {
        double eps= sqrt(((ParamX -  v_pnts[k][0])*(ParamX -  v_pnts[k][0])) +
                         ((ParamY -  v_pnts[k][1])*(ParamY -  v_pnts[k][1])));
        if (eps < distance)
        {
            distance = eps;
            j = k;
            change = true;
        }

    }
    if (change)
        return v_neighbour[j];
    else return v_neighbour[0];
}

/*! \brief Calculate the Area of the triangle, bounded by three vectors from origin. A (bad) diagram is included in source
     code */
double Routines::AreaTriangle(Base::Vector3f &a, Base::Vector3f &b, Base::Vector3f &c)
{
    /*   a
    *    x----->x  b
    *     \    /
    *      \  /
    *       \/
    *       V
    *  x c
    */
    Base::Vector3f First = b - a;
    Base::Vector3f Second = c - a;
    std::vector < std::vector<double> > Matrix(2, std::vector<double>(2));
    Matrix[0][0] = First.x;
    Matrix[1][0] = First.y;
    Matrix[0][1] = Second.x;
    Matrix[1][1] = Second.y;
    return (0.5 * det2(Matrix));
}

/*! \brief Knot Extension

 This method will extend the knots by 2. ErrPnt will tell where the knots should be inserted, NurbDegree
 and MaxCntrl will make sure that the amount of 0's and 1's are left untouched.

 The new values will be calculated here and will be inserted in Extension function
*/
void Routines::ExtendKnot(double ErrPnt, int NurbDegree, int MaxCntrl, std::vector<double> &KnotSequence)
{
    double tol_knot = 0.02;
    int ind_1 = 0;
    double new1 = 0;
    int ind_2 = 0;
    double new2 = 0;
    bool flag = false;

    if (KnotSequence.size()>40)
        tol_knot = 0.015;

    for (unsigned int i = NurbDegree; i < (KnotSequence.size() - NurbDegree); i++)
    {
        if ((KnotSequence[i] > ErrPnt) && (ErrPnt >= KnotSequence[i-1]))
        {
            ind_1 = i;
            new1 = (KnotSequence[ind_1] + KnotSequence[ind_1-1]) / 2;

        }
    }
    //START CHANGE
    if (!flag)
    {
        double mid = (double)KnotSequence.size() / 2.0;

        if (ind_1 == mid)
        {

            KnotSequence.resize(KnotSequence.size() + 2);
            //MainNurb.MaxKnotU += 2;
            for (int i = KnotSequence.size() - 1; i > ind_1; i--)
                KnotSequence[i] = KnotSequence[i-2];
            KnotSequence[ind_1] = KnotSequence[ind_1-1] + ((new1 - KnotSequence[ind_1-1]) / 2.0);
            KnotSequence[ind_1+1] = new1 +((new1 - KnotSequence[ind_1-1]) / 2.0);

        }
        else
        {
            double temp = mid - ((double)ind_1);
            ind_2 = (int)(mid + temp);
            new2 = (KnotSequence[ind_2-1] + KnotSequence[ind_2]) / 2.0;
            if (ind_1 < ind_2)
                Extension(KnotSequence,ind_1,ind_2,new1,new2);
            else
                Extension(KnotSequence,ind_2,ind_1,new2,new1);
        }
    }
    else
    {
        if (ind_1 < ind_2)
            Extension(KnotSequence,ind_1,ind_2,new1,new2);
        else
            Extension(KnotSequence,ind_2,ind_1,new2,new1);
    }
}
/*! \brief This routine is because it is done over and over again...
*/
void Routines::Extension(std::vector<double> &KnotSequence, int SmallerIndex, int LargerIndex, double SmallerIndNewValue,
                         double LargerIndNewValue)
{
    KnotSequence.resize(KnotSequence.size() + 1);

    for (int i = KnotSequence.size() - 1; i > SmallerIndex; i--)
        KnotSequence[i] = KnotSequence[i-1];

    KnotSequence[SmallerIndex] = SmallerIndNewValue;

    LargerIndex++;
    KnotSequence.resize(KnotSequence.size() + 1);

    for (int i = KnotSequence.size() - 1; i > LargerIndex; i--)
        KnotSequence[i] = KnotSequence[i-1];

    KnotSequence[LargerIndex] = LargerIndNewValue;
}