//#####################################################################
// Copyright 2004-2008, Ron Fedkiw, Eran Guendelman, Geoffrey Irving, Craig Schroeder.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
// Class INTERPOLATION_CURVE
//#####################################################################
//
// A control point's value controls the interval to the right of the control point.
//
//#####################################################################
#ifndef __INTERPOLATION_CURVE__
#define __INTERPOLATION_CURVE__

#include <Core/Arrays/ARRAY.h>
#include <Core/Data_Structures/TUPLE.h>
#include <Core/Matrices/MATRIX_POLICY.h>
#include <Core/Vectors/VECTOR_FORWARD.h>
namespace PhysBAM{

template<class T,class S> struct INTERPOLATION_CURVE_HELPER
{
    typedef S DERIVATIVE;

    static S Value(const T t_left,const T t_right,const S& u_left,const S& u_right,const T t)
    {return u_left+(t-t_left)*(u_right-u_left)/(t_right-t_left);}

    static S Difference(const S& u_left,const S& u_right)
    {return u_right-u_left;}
};

template<class T> struct INTERPOLATION_CURVE_HELPER<T,bool> // enforce constant interpolation for bools
{
    struct UNUSABLE{};
    typedef UNUSABLE DERIVATIVE;

    static bool Value(const T t_left,const T t_right,const bool u_left,const bool u_right,const T t)
    {return u_left;}
};

template<class T,class TV> struct INTERPOLATION_CURVE_HELPER<T,ROTATION<TV> >
{
    typedef typename TV::SPIN DERIVATIVE;

    static ROTATION<TV> Value(const T t_left,const T t_right,const ROTATION<TV>& u_left,const ROTATION<TV>& u_right,const T t)
    {return ROTATION<TV>::Spherical_Linear_Interpolation(u_left,u_right,(t-t_left)/(t_right-t_left));}

    static DERIVATIVE Difference(const ROTATION<TV>& u_left,const ROTATION<TV>& u_right)
    {return (u_right*u_left.Inverse()).Rotation_Vector();}
};

template<class T,class TV> struct INTERPOLATION_CURVE_HELPER<T,FRAME<TV> >
{
    typedef TWIST<TV> DERIVATIVE;

    static FRAME<TV> Value(const T t_left,const T t_right,const FRAME<TV>& u_left,const FRAME<TV>& u_right,const T t)
    {return FRAME<TV>::Interpolation(u_left,u_right,(t-t_left)/(t_right-t_left));}

    static TWIST<TV> Difference(const FRAME<TV>& u_left,const FRAME<TV>& u_right)
    {return TWIST<TV>(u_right.t-u_left.t,(u_right.r*u_left.r.Inverse()).Rotation_Vector());}
};

template<class T,class T2>
class INTERPOLATION_CURVE
{
public:
    ARRAY<std::pair<T,T2>> control_points;
    bool linear_ends=false;

    INTERPOLATION_CURVE()=default;
    INTERPOLATION_CURVE(const INTERPOLATION_CURVE&)=default;
    template<class U,class U2>
    explicit INTERPOLATION_CURVE(const INTERPOLATION_CURVE<U,U2>& ic)
        :linear_ends(ic.linear_ends)
    {
        for(const auto&c:ic.control_points)
            control_points.Append({(T)c.first,(T2)c.second});
    }
    INTERPOLATION_CURVE(INTERPOLATION_CURVE&&)=default;
    INTERPOLATION_CURVE& operator=(const INTERPOLATION_CURVE&)=default;
    INTERPOLATION_CURVE& operator=(INTERPOLATION_CURVE&&)=default;
    ~INTERPOLATION_CURVE()=default;
    
    template<class S> struct HELPER:public INTERPOLATION_CURVE_HELPER<T,S>{};
    typedef typename HELPER<T2>::DERIVATIVE DERIVATIVE;

    int Locate_Interval(const T t) const
    {
        int lo=-1,hi=control_points.m;
        while(hi-lo>1)
        {
            int m=(lo+hi)/2;
            if(t<control_points(m).first) hi=m;
            else lo=m;
        }
        return lo;
    }

    T2 Value(T t) const
    {
        if(control_points.m<2)
        {
            if(!control_points.m) return T2();
            return control_points(0).second;
        }
        int i=Locate_Interval(t);
        if(i==-1)
        {
            if(linear_ends) i=0;
            else return control_points(0).second;
        }
        else if(i==control_points.m-1)
        {
            if(linear_ends) i=control_points.m-2;
            else return control_points(i).second;
        }
        return HELPER<T2>::Value(control_points(i).first,control_points(i+1).first,control_points(i).second,control_points(i+1).second,t);
    }

    DERIVATIVE Derivative(T t) const
    {
        if(control_points.m<2) return DERIVATIVE();
        int i=Locate_Interval(t);
        if(i==-1)
        {
            if(linear_ends) i=0;
            else return DERIVATIVE();
        }
        else if(i==control_points.m-1)
        {
            if(linear_ends) i=control_points.m-2;
            else return DERIVATIVE();
        }
        return 1/(control_points(i+1).first-control_points(i).first)*HELPER<T2>::Difference(control_points(i).second,control_points(i+1).second);
    }

    void Add_Control_Point(T t,const T2& value)
    {
        control_points.Insert({t,value},Locate_Interval(t)+1);
    }

    template<class RW> void Read(UNTYPED_ISTREAM<RW>& input)
    {
        Read_Binary_Group(input,control_points,linear_ends);
    }
    template<class RW> void Write(UNTYPED_OSTREAM<RW>& output) const
    {
        Write_Binary_Group(output,control_points,linear_ends);
    }
//#####################################################################
};
}
#endif
