//#####################################################################
// Copyright 2004-2007, Ron Fedkiw, Eran Guendelman, Michael Lentine, Craig Schroeder, Tamar Shinar, Jonathan Su, Joseph Teran, Rachel Weinstein.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
// Class BALL_JOINT
//#####################################################################
#ifndef __BALL_JOINT__
#define __BALL_JOINT__

#include <Core/Math_Tools/RANGE.h>
#include <Rigids/Joints/JOINT.h>
#include <Rigids/Joints/POINT_JOINT.h>
namespace PhysBAM{

template<class TV>
class BALL_JOINT:public JOINT<TV>
{
    typedef typename TV::SCALAR T;
    typedef typename TV::SPIN T_SPIN;
    enum{dof=T_SPIN::m};
    typedef VECTOR<bool,dof> BOOL_VECTOR;
public:
    using JOINT<TV>::J;using JOINT<TV>::J_inverse;
    using JOINT<TV>::F_pj;using JOINT<TV>::F_cj;
    using JOINT<TV>::F_jp;using JOINT<TV>::F_jc;
    using JOINT<TV>::Constraint_Matrix_Helper;

    bool prismatic_component;
    bool swing_constrain=false;
    bool swing_constraint_active=false;
    TV prismatic_translation;
    RANGE<VECTOR<T,dof> > rotation_limits;
    T swing_limit_angle;
    BOOL_VECTOR constrain;
    BOOL_VECTOR constraint_active;   
    bool use_prestab_for_twist_constraint;
private:
    // twist constraint variables
    bool adjust_gamma;
    T euler_epsilon;
    ROTATION<TV> J_r_last;
    bool allow_switching;

public:
    BALL_JOINT()
        :prismatic_component(false),rotation_limits(rotation_limits.Full_Box()),use_prestab_for_twist_constraint(true),adjust_gamma(false),euler_epsilon(1e-6)
    {
        Allow_Switching(true);
    }

    virtual ~BALL_JOINT();

    void Set_Prismatic_Component_Translation(const TV& translation)
    {prismatic_translation=translation;prismatic_component=true;}

    void Use_Rotation_Constraint(const T min_input=-pi,const T max_input=pi)
    {static_assert(dof==1);constrain.x=true;rotation_limits.min_corner.x=min_input;rotation_limits.max_corner.x=max_input;}

    void Use_Twist_Constraint(const T min_input=-pi,const T max_input=pi)
    {static_assert(dof==3);constrain.x=true;rotation_limits.min_corner.x=min_input;rotation_limits.max_corner.x=max_input;}

    void Use_Phi_Constraint(const T min_input=-pi*.5,const T max_input=pi*.5)
    {static_assert(dof==3);constrain.y=true;rotation_limits.min_corner.y=min_input;rotation_limits.max_corner.y=max_input;}

    void Use_Theta_Constraint(const T min_input=-pi,const T max_input=pi)
    {static_assert(dof==3);constrain.z=true;rotation_limits.min_corner.z=min_input;rotation_limits.max_corner.z=max_input;}

    void Use_Swing_Constraint(const T swing_limit_angle_input)
    {static_assert(dof==3);swing_constrain=true;swing_limit_angle=swing_limit_angle_input;}

    void Remove_Rotation_Constraint()
    {static_assert(dof==1);constrain.x=false;rotation_limits.min_corner.x=-FLT_MAX;rotation_limits.max_corner.x=FLT_MAX;}

    void Remove_Twist_Constraint()
    {static_assert(dof==3);constrain.x=false;rotation_limits.min_corner.x=-FLT_MAX;rotation_limits.max_corner.x=FLT_MAX;}

    void Remove_Phi_Constraint()
    {static_assert(dof==3);constrain.y=false;rotation_limits.min_corner.y=-FLT_MAX;rotation_limits.max_corner.y=FLT_MAX;}

    void Remove_Theta_Constraint()
    {static_assert(dof==3);constrain.z=false;rotation_limits.min_corner.z=-FLT_MAX;rotation_limits.max_corner.z=FLT_MAX;}

    bool Adjust_Gamma() const
    {
        return adjust_gamma;
    }

//#####################################################################
    void Constrain_Relative_Angular_Velocity(const FRAME<TV>& parent_frame,T_SPIN& relative_angular_velocity) const override;
    void Constrain_Relative_Linear_Velocity(const FRAME<TV>& parent_frame,TV& relative_linear_velocity) const override;
    TV Prismatic_Component_Translation() const override;
    void Constrain_Prismatically(TV& translation) const override;
    bool Has_Angular_Constraint() const override;
    void Constrain_Angles(T_SPIN& angles_input) const override;
    void Update_State_From_Joint_Frame(const bool enforce_constraints=true) override;
    void Angular_Constraint_Matrix(const FRAME<TV>& parent_frame,const FRAME<TV>& child_frame,MATRIX_MXN<T>& constrained_matrix,MATRIX_MXN<T>* unconstrained_matrix=0) const override;
    void Set_Active(const FRAME<TV>& parent_frame,const FRAME<TV>& child_frame) override;
    void Clear_Active() override;
    void Update_Position_Based_State(const FRAME<TV>& parent_frame,const FRAME<TV>& child_frame) override;
    void Initialize_Twist_Constraint(const T min_twist,const T max_twist,const FRAME<TV>& parent_frame,const FRAME<TV>& child_frame);
    void Allow_Switching(const bool allow);

private:
    void Switch_Gamma_Constraint(const FRAME<TV>& parent_frame,const FRAME<TV>& child_frame);
    VECTOR<T,2> Project_Angles(ROTATION<TV> q_bar, T gamma_bar, T alpha, T beta) const;
    T qdist(ROTATION<TV> q_bar, T gamma_bar, T alpha, T beta) const; // qdist works
    VECTOR<T,2> qdist_gradient(ROTATION<TV> q_bar, T gamma_bar, T alpha, T beta) const;
    MATRIX<T,2> qdist_Hessian(ROTATION<TV> q_bar, T gamma_bar, T alpha, T beta) const;
    T qdist2(ROTATION<TV> q_bar, T gamma_bar, T alpha, T beta) const; // -qdist*qdist doesn't work
    VECTOR<T,2> qdist_gradient2(ROTATION<TV> q_bar, T gamma_bar, T alpha, T beta) const;
    MATRIX<T,2> qdist_Hessian2(ROTATION<TV> q_bar, T gamma_bar, T alpha, T beta) const;
    T qdist3(ROTATION<TV> q_bar, T gamma_bar, T alpha, T beta) const; // -|qdist| works
    VECTOR<T,2> qdist_gradient3(ROTATION<TV> q_bar, T gamma_bar, T alpha, T beta) const;
    MATRIX<T,2> qdist_Hessian3(ROTATION<TV> q_bar, T gamma_bar, T alpha, T beta) const;
    T signum(T x) const;
//#####################################################################
};

template<class T>
class BALL_JOINT<VECTOR<T,2> >:public POINT_JOINT<VECTOR<T,2> >
{
public:
    BALL_JOINT()
    {}
    virtual ~BALL_JOINT()
    {}

    typedef VECTOR<T,2> TV;
    typedef POINT_JOINT<VECTOR<T,2> > BASE;
    using BASE::Constrain_Relative_Linear_Velocity;
    using BASE::Constrain_Prismatically;
    using BASE::Has_Angular_Constraint;
    using BASE::Constrain_Angles;
    using BASE::Prismatic_Component_Translation;
    bool adjust_gamma;

    void Initiaize_Twist_Constraint(const T min_twist,const T max_twist,const FRAME<TV>& parent_frame,const FRAME<TV>& child_frame)
    {}

    bool Adjust_Gamma() const
    {
        return adjust_gamma;
    }


};

template<class T>
class BALL_JOINT<VECTOR<T,1> >:public JOINT<VECTOR<T,1> >
{
public:
    BALL_JOINT()
    {
        PHYSBAM_FATAL_ERROR();
    }

    virtual ~BALL_JOINT();
};
}
#endif
