//#####################################################################
// 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 POINT_JOINT
//#####################################################################
#ifndef __POINT_JOINT__
#define __POINT_JOINT__

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

template<class TV>
class POINT_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>::Constraint_Matrix_Helper;

    bool prismatic_component;
    TV prismatic_translation;
    RANGE<VECTOR<T,dof> > rotation_limits;
    BOOL_VECTOR constrain;

    POINT_JOINT()
        :prismatic_component(false),rotation_limits(rotation_limits.Full_Box())
    {}

    virtual ~POINT_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 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;}

//#####################################################################
    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;
//#####################################################################
};

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

    virtual ~POINT_JOINT();
};
}
#endif
