#pragma once
#include <cgv/math/vec.h>
#include <cgv/math/mat.h>






template <typename T>
struct bspline_curve
{
    //control points
    cgv::math::mat<T> Q;
    //degree of piecewise polynomials
    unsigned g;
    //knot vector
    cgv::math::vec<T> U;


    bspline_curve(unsigned degree,const cgv::math::mat<T>& control_points,
        const cgv::math::mat<T>& knots)
    {
        Q = control_points;
        U = knots;
        g = degree;

    }

    ///returns the number of control points
    unsigned num_control_points()
    {
        return Q.ncols();
    }

    bspline_curve(unsigned degree,const cgv::math::mat<T>& control_points, bool interpolate_endpoints=true)
    {
        Q = control_points;
        g = degree;

        unsigned n =num_control_points();

        U.resize(n+1+g);

        if(interpolate_endpoints)
        {
            //student begin
            U.resize(n+1+g*2);
            for(unsigned i=0; i<g+1;i++) {
                U(i) = 0;
            }
            for(unsigned i=g+1; i< g+1+n; i++) {
                U(i+g) = i;
            }
            for(unsigned i=0; i<g; i++) {
                U(i+g+1+n) = g;
            }
            //student end

        }
        else
        {
            for(unsigned i =0; i < num_control_points()+g+1;i++)
            {
                U(i)=i;
            }

        }




    }




    //i... zero-based index of control point
    //g... degree of spline
    //t... position to evaluate
    //the knot vector is stored in U
    T cox_de_boor(int i, int g, const T& t)
    {
        //student begin
        if(g == 0) {
            if(U[i] <= g && g < U[i + 1] && U[i] < U[i + 1]) {
                return 1;
            }
            return 0;
        }
        return cox_de_boor(i, g - 1, t) * (t - U[i]) / (U[i + g] - U[i])
            + cox_de_boor(i + 1, g - 1, t) * (U[i + 1 + g] - t)/(U[i + 1 + g] - U[i + 1]);
        //student end
    }




    //sample basis at sample position t
    cgv::math::vec<T> sample_basis(const T& t)
    {
        cgv::math::vec<T> v(num_control_points());
        for(unsigned i = 0; i < num_control_points(); i++)
            v(i) = cox_de_boor(i,g,t);


        return v;
    }

    //sample basis at multiple sample positions t
    cgv::math::mat<T> sample_basis(const cgv::math::vec<T>& ts)
    {
        cgv::math::mat<T> vs(num_control_points(),ts.size());

        for(unsigned j = 0; j < ts.size(); j++)
            vs.set_col(j,sample_basis(ts(j)));
        return vs;
    }

    //returns a curve point at sample position t
    cgv::math::vec<T> sample_curve(const T& t)
    {
        return Q*sample_basis(t);
    }

    //returns multiple curve points at sample positions t
    cgv::math::mat<T> sample_curve(const cgv::math::vec<T>& ts)
    {
        return Q*sample_basis(ts);
    }


    /// helper method to calculate the affine combination of x and y
    static cgv::math::vec<T> aff(const T& t,const cgv::math::vec<T>& x,const cgv::math::vec<T>& y,
        const T& a,const T& b)
    {
        if (a==b)
            return x;

        return  ( 1.0 / (b-a) ) * ( (b-t)*x + (t-a)*y);
    }

    ///r..recursion depth,
    ///t..sample position,
    ///i..control point index
    cgv::math::vec<T> de_boor(int r,const T& t, int i)
    {
        //student begin
        if(r==0) {
            return Q.col(i);
        }
        T a = (t - U[i]) / (U[i+num_control_points()+1-g] - U[i]);
        return (1-a)*de_boor(r-1, t, i) + a*de_boor(r-1, t, i+1);
        //student end
    }

    cgv::math::vec<T> sample_curve_de_boor(const T& t)
    {
        assert (g < num_control_points() );

        int i = 0;
        while(t >= U(i+1)  && i+1 < U.size())
            i++;

        return (de_boor(g,t, i-g));


    }


    ///ts..sample positions
    cgv::math::mat<T> sample_curve_de_boor(const cgv::math::vec<T>& ts)
    {
        cgv::math::mat<T> S(Q.nrows(),ts.size());
        for(unsigned i = 0; i < ts.size();i++)
            S.set_col(i,sample_curve_de_boor(ts(i)));

        return S;
    }

    //creates a vector of sample positions containing num_samples values
    cgv::math::vec<T> create_samples(unsigned num_samples)
    {

        T start = U(g);
        T end = U(U.size()-1-g)-0.00001;
        //assert(start < end);


        return cgv::math::lin_space<double>(start, end, num_samples);
    }

};