//#####################################################################
// Copyright 2004-2024, Yitong Dai, 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.
//#####################################################################
#include <Rigids/Joints/BALL_JOINT.h>
#include <Core/Matrices/MATRIX.h>
#include <Core/Matrices/MATRIX_MXN.h>
#include <Core/Nonlinear_Equations/COORDINATE_DESCENT.h>
using namespace PhysBAM;
//#####################################################################
// Destructor
//#####################################################################
template<class TV> BALL_JOINT<TV>::
~BALL_JOINT()
{}
//#####################################################################
// Destructor
//#####################################################################
template<class T> BALL_JOINT<VECTOR<T,1> >::
~BALL_JOINT()
{}
//#####################################################################
// Function Constrain_Relative_Linear_Velocity
//#####################################################################
template<class TV> void BALL_JOINT<TV>::
Constrain_Relative_Angular_Velocity(const FRAME<TV>& parent_frame,T_SPIN& relative_angular_velocity) const
{
    // TODO: this won't work if we want restitution
    // in the case of restitution, we need to be able to specific C_a * relative_angular_velocity
    relative_angular_velocity=T_SPIN();
}
//#####################################################################
// Function Constrain_Relative_Linear_Velocity
//#####################################################################
template<class TV> void BALL_JOINT<TV>::
Constrain_Relative_Linear_Velocity(const FRAME<TV>& parent_frame,TV& relative_linear_velocity) const
{
    relative_linear_velocity=TV();
}
//#####################################################################
// Function Prismatic_Component_Translation
//#####################################################################
template<class TV> TV BALL_JOINT<TV>::
Prismatic_Component_Translation() const
{
    return prismatic_translation;
}
//#####################################################################
// Function Constrain_Prismatically
//  This is used by prestabilization to compute the target joint state
//#####################################################################
template<class TV> void BALL_JOINT<TV>::
Constrain_Prismatically(TV& translation) const
{
    translation=prismatic_translation;
}
//#####################################################################
// Function Has_Angular_Constraint
//#####################################################################
template<class TV> bool BALL_JOINT<TV>::
Has_Angular_Constraint() const
{
    bool result =constraint_active.Contains(true)||swing_constraint_active;
    return use_prestab_for_twist_constraint && result;
}
//#####################################################################
// Function Constrain_Angles
//  This is used by prestabilization to compute the target joint state
//#####################################################################
template<class TV> void BALL_JOINT<TV>::
Constrain_Angles(T_SPIN& angles_input) const
{
    // TODO: implement the swing cone limits for the swing part
    // currently, there will be drift for the swing constraint
    // project onto the swing limit cone to get alpha_new, beta_new
    // set alpha= alpha_new, beta=beta_new

//    for(int i=0;i<T_SPIN::m;i++) angles_input(i)=wrap(angles_input(i),(T)-pi,pi_v<T>);

    // get alpha, beta, gamma
    T gamma=angles_input.x;
    T beta=angles_input.y;
    T alpha=angles_input.z;

    // adjust gamma
    T gamma_offset=0;
    if(allow_switching && adjust_gamma){
        T sign=(gamma >= 0)?-1:1;
        gamma_offset=sign*pi;
    }

    if((gamma+gamma_offset < rotation_limits.min_corner.x || gamma+gamma_offset > rotation_limits.max_corner.x))
    {
        // find the target gamma
        T gamma_bar = clamp(gamma+gamma_offset,rotation_limits.min_corner.x,rotation_limits.max_corner.x);
        VECTOR<T,2> ab = Project_Angles(ROTATION<TV>::From_Euler_Angles(angles_input),gamma_bar-gamma_offset,alpha,beta);
        alpha=ab(0);
        beta=ab(1);
        gamma=gamma_bar-gamma_offset;

        if(allow_switching && (beta < -pi/2 || beta > pi/2)){
            LOG::cout<<"TTS beta/pi = "<<beta/pi<<std::endl;
        }
    }

    angles_input.x=gamma;
    angles_input.y=beta;
    angles_input.z=alpha;

    // alpha: [-pi,pi], beta: [-pi/2,pi/2], gamma: [-pi,pi]
}
//#####################################################################
// Function Update_State_From_Joint_Frame
//#####################################################################
template<class TV> void BALL_JOINT<TV>::
Update_State_From_Joint_Frame(const bool enforce_constraints)
{
    if(enforce_constraints){
        T_SPIN angles=J.r.Euler_Angles();
        Constrain_Angles(angles);
        J.r=ROTATION<TV>::From_Euler_Angles(angles);
        Constrain_Prismatically(J.t);}
    J_inverse=J.Inverse();
}
//#####################################################################
// Function Angular_Constraint_Matrix
//#####################################################################
template<class TV> void BALL_JOINT<TV>::
Angular_Constraint_Matrix(const FRAME<TV>& parent_frame,const FRAME<TV>& child_frame,MATRIX_MXN<T>& constrained_matrix,MATRIX_MXN<T>* unconstrained_matrix) const
{
    FRAME<TV> frame_J=F_jp()*parent_frame.Inverse()*child_frame*F_cj();

    int constrained_axes=constraint_active.Number_True();
    if(swing_constraint_active) constrained_axes++;
    constrained_matrix.Resize(TV::m,constrained_axes);

    if constexpr(TV::m==3)
    {
        MATRIX<T,TV::m> R=frame_J.r.Rotation_Matrix();
        int constraint_counters=0;
        if(constraint_active.x)
        {
            TV j_twist;
            T bottom=R(2,1)*R(2,1)+R(2,2)*R(2,2);
            j_twist(0)=(R(1,1)*R(2,2)-R(1,2)*R(2,1))/bottom;
            j_twist(1)=(R(0,2)*R(2,1)-R(0,1)*R(2,2))/bottom;
    
            // (R_wj J^t)^t = J R_jw
            TV j_twist_world=parent_frame.r*F_pj().r*j_twist;

            constrained_matrix.Set_Column(constraint_counters,j_twist_world);
            constraint_counters++;
        }
        if(swing_constraint_active)
        {
            TV x_axis(1,0,0);
            TV j_swing=(R*x_axis).Cross(x_axis);

            TV j_swing_world=parent_frame.r*F_pj().r*j_swing;
            constrained_matrix.Set_Column(constraint_counters,j_swing_world);
            constraint_counters++;
        }
    }
}
//#####################################################################
// Function Update_Position_Based_State
//#####################################################################
template<class TV> void BALL_JOINT<TV>::
Update_Position_Based_State(const FRAME<TV>& parent_frame,const FRAME<TV>& child_frame)
{
    if(allow_switching)
        Switch_Gamma_Constraint(parent_frame,child_frame);
}
//#####################################################################
// Function Set_Active
//#####################################################################
template<class TV> void BALL_JOINT<TV>::
Set_Active(const FRAME<TV>& parent_frame,const FRAME<TV>& child_frame)
{
    FRAME<TV> J_current = this->frame_jp*parent_frame.Inverse()*child_frame*this->frame_cj;
    T_SPIN angles = J_current.r.Euler_Angles();

    Update_Position_Based_State(parent_frame,child_frame);

    // adjust gamma
    if(adjust_gamma){
        T sign=(angles.x >= 0)?-1:1;
        angles.x += sign*pi;
    }        

    for(int i=0;i<T_SPIN::m;i++){
        constraint_active(i)=constrain(i)
            && (rotation_limits.max_corner(i) <= rotation_limits.min_corner(i) 
                || angles(i) < rotation_limits.min_corner(i) 
                || angles(i) > rotation_limits.max_corner(i));

//        if(constraint_active(i))
//            std::cout<<"TTS found an active constraint ("<<i<<") "<<angles(i)<<" not in "<<"("<<rotation_limits.min_corner(i) <<","<<rotation_limits.max_corner(i)<<")"<<std::endl;
    }

    // disable the gamma constraint if inside the singular region
    T abs_beta = abs(angles(1));
    bool enable_gamma_constraint=(pi/2 - abs_beta > euler_epsilon);
    constraint_active(0) &= enable_gamma_constraint;

    
    T_SPIN clamped_angles=angles;
    clamped_angles(0)=0;
    ROTATION<TV> rotation_swing=ROTATION<TV>::From_Euler_Angles(clamped_angles);
    TV x_axis(1,0,0);
    TV rotated_x=rotation_swing.Rotate(x_axis);
    T current_angle=x_axis.Dot(rotated_x);
    if(current_angle<cos(swing_limit_angle)&&swing_constrain)
    {
        swing_constraint_active=true;
    }

//    std::cout<<"TTS active state is "<<constraint_active(0)<<","<<constraint_active(1)<<","<<constraint_active(2)<<","<<swing_constraint_active<<std::endl;

}
//#####################################################################
// Function Allow_Switching
//#####################################################################
template<class TV> void BALL_JOINT<TV>::
Allow_Switching(const bool allow)
{
    allow_switching=allow;
}
//#####################################################################
// Function Clear_Active
//#####################################################################
template<class TV> void BALL_JOINT<TV>::
Clear_Active()
{
    constraint_active.Fill(false);
    swing_constraint_active=false;
}
//#####################################################################
// Function Clear_Active
//#####################################################################
template<class TV> void BALL_JOINT<TV>::
Initialize_Twist_Constraint(const T min_twist,const T max_twist,const FRAME<TV>& parent_frame,const FRAME<TV>& child_frame)
{
    this->Use_Twist_Constraint(min_twist,max_twist);
    FRAME<TV> J_current = this->frame_jp*parent_frame.Inverse()*child_frame*this->frame_cj;
    J_r_last = J_current.r;
}
//#####################################################################
// Function Switch_Gamma_Constraint
//#####################################################################
template<class TV> void BALL_JOINT<TV>::
Switch_Gamma_Constraint(const FRAME<TV>& parent_frame,const FRAME<TV>& child_frame)
{
    FRAME<TV> J_current = this->frame_jp*parent_frame.Inverse()*child_frame*this->frame_cj;
    T_SPIN angles = J_current.r.Euler_Angles();
    T abs_beta = abs(angles(1));
    
    // if far enough from singular points,
    // then check whether gamma needs adjusting
    if(pi/2 - abs_beta > euler_epsilon)
    {
        // compute the mapping from omega to angles dot 
        // based on previous orientation
        T_SPIN angles_prev = J_r_last.Euler_Angles();
        T alpha_prev = angles_prev(2);
        TV omega_to_beta_dot(-sin(alpha_prev), cos(alpha_prev), 0);

        // predict beta based on change in orientation
        ROTATION<TV> rot_diff=J_current.r*J_r_last.Inverse();
        TV rot_diff_vec=rot_diff.Rotation_Vector();
        T beta_diff = omega_to_beta_dot.Dot(rot_diff_vec);
        T beta_predicted = angles_prev.y + beta_diff;

        if(beta_predicted > pi/2 || beta_predicted < -pi/2)
        {
            // beta went outside valid range
            // toggle adjust_gamma
            adjust_gamma = !adjust_gamma;
            LOG::cout << "============Ball Joint Switch (predicted beta)==================: \n" << angles.y << "  -> "<<beta_predicted << std::endl;
        }

        J_r_last=J_current.r;
    }
    else
    {
        LOG::cout << "-----Ball Joint Inside singularity region" << std::endl;
    }
    
}
//#####################################################################
// Function Project_Angles
//#####################################################################
template<class TV> VECTOR<typename TV::SCALAR,2> BALL_JOINT<TV>::
Project_Angles(ROTATION<TV> q_bar, T gamma_bar, T alpha, T beta) const
{
    T a=alpha;
    T b=beta;

    VECTOR<T,2> g;

    T tol=1e-6;
    T step_size=.5;
    int iter_count=0;

    bool newton=false; // doesn't work - not downhill
    int max_newton_iter=5;
    int max_iter=1500;

    bool conv=false;
    if(newton){
        while(!conv && iter_count<max_newton_iter)
        {
            g=qdist_gradient(q_bar,gamma_bar, a, b);
            MATRIX<T,2> H=qdist_Hessian(q_bar,gamma_bar, a, b);
            VECTOR<T,2> s = H.Inverse_Times(g);
            if(Dot_Product(s,g)>0)
                std::cout<<"("<<iter_count+1<<") wrong dir"<<std::endl;
            a -= s(0);
            b -= s(1);
            conv=(g.Magnitude()<tol);
            iter_count++;
        }
        if(!conv){
            LOG::cout<<"NEWTON did not converge in "<<max_newton_iter<<" iterations"<<std::endl;
            a = alpha;
            b = beta;
        }
    }
    iter_count=0;
    while(!conv && iter_count<max_iter)
    {
        g=qdist_gradient(q_bar,gamma_bar, a, b);
        a -= step_size*g(0);
        b -= step_size*g(1);
        conv=(g.Magnitude()<tol);
        iter_count++;
    }
    if(!conv)
        LOG::cout<<"BALL_JOINT::Project_Angles NOT converged after "<<iter_count<<" iterations, |g| = "<<abs(g.Magnitude())<<std::endl;


    a=wrap((T)a,(T)-pi,(T)pi);
    b=wrap((T)b,(T)-pi,(T)pi);
    if(allow_switching && (b < -pi/2 || b > pi/2))
        PHYSBAM_WARNING("beta out of range");  // if beta is out of the range, then need to toggle adjust_beta again?

    return {a,b};
}
//#####################################################################
// Function qdist
//#####################################################################
template<class TV> TV::SCALAR BALL_JOINT<TV>::
qdist(ROTATION<TV> q_bar, T gamma_bar, T alpha, T beta) const
{
    T a = alpha;
    T b = beta;
    T g_bar = gamma_bar;
    T q0=q_bar.q.s;
    T q1=q_bar.q.v.x;
    T q2=q_bar.q.v.y;
    T q3=q_bar.q.v.z;

    T f=
        ((sin(0.5*b)*q2+q0*cos(0.5*b))*cos(0.5*a)-sin(0.5*a)*(sin(0.5*b)*q1-q3*cos(0.5*b)))*cos(0.5*g_bar)
        +sin(0.5*g_bar)*((-sin(0.5*b)*q3+q1*cos(0.5*b))*cos(0.5*a)+sin(0.5*a)*(sin(0.5*b)*q0+q2*cos(0.5*b)));

    return f;
}
//#####################################################################
// Function qdist_gradient
//#####################################################################
template<class TV> VECTOR<typename TV::SCALAR,2> BALL_JOINT<TV>::
qdist_gradient(ROTATION<TV> q_bar, T gamma_bar, T alpha, T beta) const
{
    T a = alpha;
    T b = beta;
    T g_bar = gamma_bar;
    T q0=q_bar.q.s;
    T q1=q_bar.q.v.x;
    T q2=q_bar.q.v.y;
    T q3=q_bar.q.v.z;

    T ga = 0.5*((-sin(0.5*b)*q1+q3*cos(0.5*b))*cos(0.5*a)-(sin(0.5*b)*q2+q0*cos(0.5*b))*sin(0.5*a))*cos(0.5*g_bar)+0.5*sin(0.5*g_bar)*((sin(0.5*b)*q3-q1*cos(0.5*b))*sin(0.5*a)+cos(0.5*a)*(sin(0.5*b)*q0+q2*cos(0.5*b)));
    T gb = 0.5*((q2*cos(0.5*b)-sin(0.5*b)*q0)*cos(0.5*a)-sin(0.5*a)*(q1*cos(0.5*b)+sin(0.5*b)*q3))*cos(0.5*g_bar)-0.5*sin(0.5*g_bar)*((q3*cos(0.5*b)+sin(0.5*b)*q1)*cos(0.5*a)+sin(0.5*a)*(-q0*cos(0.5*b)+sin(0.5*b)*q2));

    return {ga,gb};
}
//#####################################################################
// Function qdist_Hessian
//#####################################################################
template<class TV> MATRIX<typename TV::SCALAR,2> BALL_JOINT<TV>::
qdist_Hessian(ROTATION<TV> q_bar, T gamma_bar, T alpha, T beta) const
{
    MATRIX<T,2> H;

    T a = alpha;
    T b = beta;
    T g_bar = gamma_bar;
    T q0=q_bar.q.s;
    T q1=q_bar.q.v.x;
    T q2=q_bar.q.v.y;
    T q3=q_bar.q.v.z;

    H(0, 0) = 0.25*((-sin(0.5*b)*q2-q0*cos(0.5*b))*cos(0.5*a)+sin(0.5*a)*(sin(0.5*b)*q1-q3*cos(0.5*b)))*cos(0.5*g_bar)
        -0.25*sin(0.5*g_bar)*((-sin(0.5*b)*q3+q1*cos(0.5*b))*cos(0.5*a)+sin(0.5*a)*(sin(0.5*b)*q0+q2*cos(0.5*b)));
    H(0, 1) = 0.25*((-q2*cos(0.5*b)+sin(0.5*b)*q0)*sin(0.5*a)+(-q1*cos(0.5*b)-sin(0.5*b)*q3)*cos(0.5*a))*cos(0.5*g_bar)
        +0.25*sin(0.5*g_bar)*((q3*cos(0.5*b)+sin(0.5*b)*q1)*sin(0.5*a)+cos(0.5*a)*(q0*cos(0.5*b)-sin(0.5*b)*q2));
    H(1, 0) = H(0,1);
    H(1, 1) = 0.25*((-sin(0.5*b)*q2-q0*cos(0.5*b))*cos(0.5*a)+sin(0.5*a)*(sin(0.5*b)*q1-q3*cos(0.5*b)))*cos(0.5*g_bar)
        -0.25*sin(0.5*g_bar)*((-sin(0.5*b)*q3+q1*cos(0.5*b))*cos(0.5*a)+sin(0.5*a)*(sin(0.5*b)*q0+q2*cos(0.5*b)));

    return H;
}
//#####################################################################
// Function qdist
//#####################################################################
template<class TV> TV::SCALAR BALL_JOINT<TV>::
qdist2(ROTATION<TV> q_bar, T gamma_bar, T alpha, T beta) const
{
    T a = alpha;
    T b = beta;
    T g_bar = gamma_bar;
    T q0=q_bar.q.s;
    T q1=q_bar.q.v.x;
    T q2=q_bar.q.v.y;
    T q3=q_bar.q.v.z;
    T one_half=0.5;

    T qdot=((-sin(one_half*b)*q2-q0*cos(one_half*b))*cos(one_half*a)+sin(one_half*a)*(sin(one_half*b)*q1-q3*cos(one_half*b)))*cos(one_half*g_bar)-sin(one_half*g_bar)*((-sin(one_half*b)*q3+q1*cos(one_half*b))*cos(one_half*a)+sin(one_half*a)*(sin(one_half*b)*q0+q2*cos(one_half*b)));
        
    T f=-qdot*qdot;

    return f;
}
//#####################################################################
// Function qdist_gradient
//#####################################################################
template<class TV> VECTOR<typename TV::SCALAR,2> BALL_JOINT<TV>::
qdist_gradient2(ROTATION<TV> q_bar, T gamma_bar, T alpha, T beta) const
{
    T a = alpha;
    T b = beta;
    T g_bar = gamma_bar;
    T q0=q_bar.q.s;
    T q1=q_bar.q.v.x;
    T q2=q_bar.q.v.y;
    T q3=q_bar.q.v.z;
    T one_half=0.5;

    T ga=-(((-sin(one_half*b)*q2-q0*cos(one_half*b))*cos(one_half*a)+sin(one_half*a)*(sin(one_half*b)*q1-q3*cos(one_half*b)))*cos(one_half*g_bar)-sin(one_half*g_bar)*((-sin(one_half*b)*q3+q1*cos(one_half*b))*cos(one_half*a)+sin(one_half*a)*(sin(one_half*b)*q0+q2*cos(one_half*b))))*((cos(one_half*a)*(sin(one_half*b)*q1-q3*cos(one_half*b))+sin(one_half*a)*(sin(one_half*b)*q2+q0*cos(one_half*b)))*cos(one_half*g_bar)-(cos(one_half*a)*(sin(one_half*b)*q0+q2*cos(one_half*b))+sin(one_half*a)*(sin(one_half*b)*q3-q1*cos(one_half*b)))*sin(one_half*g_bar));

    T gb=-(((-sin(one_half*b)*q2-cos(one_half*b)*q0)*cos(one_half*a)+sin(one_half*a)*(sin(one_half*b)*q1-cos(one_half*b)*q3))*cos(one_half*g_bar)-sin(one_half*g_bar)*((-sin(one_half*b)*q3+q1*cos(one_half*b))*cos(one_half*a)+sin(one_half*a)*(sin(one_half*b)*q0+q2*cos(one_half*b))))*(((-q2*cos(one_half*b)+sin(one_half*b)*q0)*cos(one_half*a)+sin(one_half*a)*(q1*cos(one_half*b)+sin(one_half*b)*q3))*cos(one_half*g_bar)+sin(one_half*g_bar)*((cos(one_half*b)*q3+sin(one_half*b)*q1)*cos(one_half*a)+sin(one_half*a)*(-cos(one_half*b)*q0+sin(one_half*b)*q2)));

    return {ga,gb};
}
//#####################################################################
// Function qdist_Hessian
//#####################################################################
template<class TV> MATRIX<typename TV::SCALAR,2> BALL_JOINT<TV>::
qdist_Hessian2(ROTATION<TV> q_bar, T gamma_bar, T alpha, T beta) const
{
    T a = alpha;
    T b = beta;
    T g_bar = gamma_bar;
    T q0=q_bar.q.s;
    T q1=q_bar.q.v.x;
    T q2=q_bar.q.v.y;
    T q3=q_bar.q.v.z;
    T one_half=0.5;
    T one_quarter=0.25;

    MATRIX<T,2> H;

    H(0,0)=one_half*((((-q1-q2)*cos(one_half*a)+sin(one_half*a)*(q1-q2))*sin(one_half*g_bar)+((-q0-q3)*cos(one_half*a)+sin(one_half*a)*(q0-q3))*cos(one_half*g_bar))*cos(one_half*b)+sin(one_half*b)*(((-q0+q3)*cos(one_half*a)-sin(one_half*a)*(q0+q3))*sin(one_half*g_bar)+((q1-q2)*cos(one_half*a)+sin(one_half*a)*(q1+q2))*cos(one_half*g_bar)))*(sin(one_half*b)*(((q0+q3)*cos(one_half*a)-sin(one_half*a)*(q0-q3))*sin(one_half*g_bar)+cos(one_half*g_bar)*((-q1-q2)*cos(one_half*a)+sin(one_half*a)*(q1-q2)))-cos(one_half*b)*(((q1-q2)*cos(one_half*a)+sin(one_half*a)*(q1+q2))*sin(one_half*g_bar)+((q0-q3)*cos(one_half*a)+sin(one_half*a)*(q0+q3))*cos(one_half*g_bar)));
    H(0,0)=one_quarter*(((-q0*q0+q1*q1-q2*q2+q3*q3)*sin(g_bar)+2*cos(g_bar)*(q0*q1-q2*q3))*cos(a)+2*((-q0*q3+q1*q2)*sin(g_bar)+cos(g_bar)*(q0*q2+q1*q3))*sin(a))*cos(b)-one_quarter*sin(b)*((-2*q0*q3-2*q1*q2)*cos(a)+sin(a)*(q0*q0+q1*q1-q2*q2-q3*q3));
    H(1,0)=H(0,1);
    H(1,1)=one_half*((((-q1+q3)*cos(one_half*a)-sin(one_half*a)*(q0+q2))*sin(one_half*g_bar)+((-q0-q2)*cos(one_half*a)+sin(one_half*a)*(q1-q3))*cos(one_half*g_bar))*cos(one_half*b)+sin(one_half*b)*(((q1+q3)*cos(one_half*a)-(q0-q2)*sin(one_half*a))*sin(one_half*g_bar)+cos(one_half*g_bar)*((q0-q2)*cos(one_half*a)+sin(one_half*a)*(q1+q3))))*(sin(one_half*b)*(((-q1+q3)*cos(one_half*a)-sin(one_half*a)*(q0+q2))*sin(one_half*g_bar)+((-q0-q2)*cos(one_half*a)+sin(one_half*a)*(q1-q3))*cos(one_half*g_bar))-cos(one_half*b)*(((q1+q3)*cos(one_half*a)-(q0-q2)*sin(one_half*a))*sin(one_half*g_bar)+cos(one_half*g_bar)*((q0-q2)*cos(one_half*a)+sin(one_half*a)*(q1+q3))));

    return H;
}
//#####################################################################
// Function qdist
//#####################################################################
template<class TV> TV::SCALAR BALL_JOINT<TV>::
qdist3(ROTATION<TV> q_bar, T gamma_bar, T alpha, T beta) const
{
    T a = alpha;
    T b = beta;
    T g_bar = gamma_bar;
    T q0=q_bar.q.s;
    T q1=q_bar.q.v.x;
    T q2=q_bar.q.v.y;
    T q3=q_bar.q.v.z;

    T f=-1.*abs(((sin(.5000000000*b)*q2+q0*cos(.5000000000*b))*cos(.5000000000*a)-1.*sin(.5000000000*a)*(sin(.5000000000*b)*q1-1.*q3*cos(.5000000000*b)))*cos(.5000000000*g_bar)+((-1.*sin(.5000000000*b)*q3+q1*cos(.5000000000*b))*cos(.5000000000*a)+sin(.5000000000*a)*(sin(.5000000000*b)*q0+q2*cos(.5000000000*b)))*sin(.5000000000*g_bar));

    return f;
}
//#####################################################################
// Function qdist_gradient
//#####################################################################
template<class TV> VECTOR<typename TV::SCALAR,2> BALL_JOINT<TV>::
qdist_gradient3(ROTATION<TV> q_bar, T gamma_bar, T alpha, T beta) const
{
    T a = alpha;
    T b = beta;
    T g_bar = gamma_bar;
    T q0=q_bar.q.s;
    T q1=q_bar.q.v.x;
    T q2=q_bar.q.v.y;
    T q3=q_bar.q.v.z;

    T sn = signum(((-1.*cos(.5000000000*g_bar)*q1+sin(.5000000000*g_bar)*q0)*sin(.5000000000*a)+(q2*cos(.5000000000*g_bar)-1.*q3*sin(.5000000000*g_bar))*cos(.5000000000*a))*sin(.5000000000*b)+((cos(.5000000000*g_bar)*q0+sin(.5000000000*g_bar)*q1)*cos(.5000000000*a)+sin(.5000000000*a)*(cos(.5000000000*g_bar)*q3+sin(.5000000000*g_bar)*q2))*cos(.5000000000*b));

    T ga=.5000000000*sn*(((sin(.5000000000*b)*q2+q0*cos(.5000000000*b))*sin(.5000000000*a)+cos(.5000000000*a)*(sin(.5000000000*b)*q1-1.*q3*cos(.5000000000*b)))*cos(.5000000000*g_bar)-1.*sin(.5000000000*g_bar)*(cos(.5000000000*a)*(sin(.5000000000*b)*q0+q2*cos(.5000000000*b))+sin(.5000000000*a)*(sin(.5000000000*b)*q3-1.*q1*cos(.5000000000*b))));

    T gb=.5000000000*sn*(((-1.*q2*cos(.5000000000*b)+sin(.5000000000*b)*q0)*cos(.5000000000*a)+sin(.5000000000*a)*(q1*cos(.5000000000*b)+sin(.5000000000*b)*q3))*cos(.5000000000*g_bar)+((q3*cos(.5000000000*b)+sin(.5000000000*b)*q1)*cos(.5000000000*a)+sin(.5000000000*a)*(-1.*q0*cos(.5000000000*b)+sin(.5000000000*b)*q2))*sin(.5000000000*g_bar));

    return {ga,gb};
}
//#####################################################################
// Function qdist_Hessian
//#####################################################################
template<class TV> MATRIX<typename TV::SCALAR,2> BALL_JOINT<TV>::
qdist_Hessian3(ROTATION<TV> q_bar, T gamma_bar, T alpha, T beta) const
{
    MATRIX<T,2> H;

    T a = alpha;
    T b = beta;
    T g_bar = gamma_bar;
    T q0=q_bar.q.s;
    T q1=q_bar.q.v.x;
    T q2=q_bar.q.v.y;
    T q3=q_bar.q.v.z;

    T one_half=0.5;
    T one_quarter=0.25;

    T sn0=signum(((q0*sin(one_half*g_bar)-q1*cos(one_half*g_bar))*sin(one_half*a)+(-q3*sin(one_half*g_bar)+q2*cos(one_half*g_bar))*cos(one_half*a))*sin(one_half*b)+((cos(one_half*g_bar)*q0+sin(one_half*g_bar)*q1)*cos(one_half*a)+sin(one_half*a)*(cos(one_half*g_bar)*q3+sin(one_half*g_bar)*q2))*cos(one_half*b));
//    T sn1=signum(1,((q0*sin(one_half*g_bar)-q1*cos(one_half*g_bar))*sin(one_half*a)+(-q3*sin(one_half*g_bar)+q2*cos(one_half*g_bar))*cos(one_half*a))*sin(one_half*b)+((cos(one_half*g_bar)*q0+sin(one_half*g_bar)*q1)*cos(one_half*a)+sin(one_half*a)*(cos(one_half*g_bar)*q3+sin(one_half*g_bar)*q2))*cos(one_half*b));
    T sn1=0.;

    T t1=(((-sin(one_half*g_bar)*q2-cos(one_half*g_bar)*q3)*cos(one_half*a)+sin(one_half*a)*(cos(one_half*g_bar)*q0+sin(one_half*g_bar)*q1))*cos(one_half*b)
        +((-q0*sin(one_half*g_bar)+q1*cos(one_half*g_bar))*cos(one_half*a)+sin(one_half*a)*(-q3*sin(one_half*g_bar)+q2*cos(one_half*g_bar)))*sin(one_half*b));
    
    T t2=(((q3*sin(one_half*g_bar)-q2*cos(one_half*g_bar))*cos(one_half*a)+sin(one_half*a)*(-q0*sin(one_half*g_bar)+q1*cos(one_half*g_bar)))*cos(one_half*b)
    +((cos(one_half*g_bar)*q0+sin(one_half*g_bar)*q1)*cos(one_half*a)+sin(one_half*a)*(cos(one_half*g_bar)*q3+sin(one_half*g_bar)*q2))*sin(one_half*b));

    H(0,0) = -one_quarter*t1*t1*sn1
        -one_quarter*(((-sin(one_half*g_bar)*q1-cos(one_half*g_bar)*q0)*cos(one_half*a)-sin(one_half*a)*(cos(one_half*g_bar)*q3+sin(one_half*g_bar)*q2))*cos(one_half*b)+((q3*sin(one_half*g_bar)-q2*cos(one_half*g_bar))*cos(one_half*a)+sin(one_half*a)*(-q0*sin(one_half*g_bar)+q1*cos(one_half*g_bar)))*sin(one_half*b))*sn0;

    H(0,1) = -one_quarter*(((-sin(one_half*g_bar)*q2-cos(one_half*g_bar)*q3)*cos(one_half*b)+sin(one_half*b)*(-q0*sin(one_half*g_bar)+q1*cos(one_half*g_bar)))*cos(one_half*a)
        +((cos(one_half*g_bar)*q0+sin(one_half*g_bar)*q1)*cos(one_half*b)+sin(one_half*b)*(-q3*sin(one_half*g_bar)+q2*cos(one_half*g_bar)))*sin(one_half*a))*(((q3*sin(one_half*g_bar)-q2*cos(one_half*g_bar))*cos(one_half*b)+sin(one_half*b)*(cos(one_half*g_bar)*q0+sin(one_half*g_bar)*q1))*cos(one_half*a)+sin(one_half*a)*((-q0*sin(one_half*g_bar)+q1*cos(one_half*g_bar))*cos(one_half*b)+sin(one_half*b)*(cos(one_half*g_bar)*q3+sin(one_half*g_bar)*q2)))*sn1
        -one_quarter*(((q0*sin(one_half*g_bar)-q1*cos(one_half*g_bar))*cos(one_half*b)-sin(one_half*b)*(cos(one_half*g_bar)*q3+sin(one_half*g_bar)*q2))*cos(one_half*a)+sin(one_half*a)*((q3*sin(one_half*g_bar)-q2*cos(one_half*g_bar))*cos(one_half*b)+sin(one_half*b)*(cos(one_half*g_bar)*q0+sin(one_half*g_bar)*q1)))*sn0;

    H(1,0)=H(0,1);
    
    H(1,1)=-one_quarter*t2*t2*sn1
        -one_quarter*(((-sin(one_half*g_bar)*q1-cos(one_half*g_bar)*q0)*cos(one_half*a)-sin(one_half*a)*(cos(one_half*g_bar)*q3+sin(one_half*g_bar)*q2))*cos(one_half*b)+((q3*sin(one_half*g_bar)-q2*cos(one_half*g_bar))*cos(one_half*a)+sin(one_half*a)*(-q0*sin(one_half*g_bar)+q1*cos(one_half*g_bar)))*sin(one_half*b))*sn0;

    return H;
}
//#####################################################################
// Function signum
//#####################################################################
template<class TV> typename TV::SCALAR BALL_JOINT<TV>::
signum(T x) const
{
    if(!x) return 1;
    return x/abs(x);
}
//#####################################################################
namespace PhysBAM{
#define INSTANTIATION_HELPER(T,d,s) \
    template BALL_JOINT<VECTOR<T,d> >::~BALL_JOINT(); \
    template void BALL_JOINT<VECTOR<T,d> >::Constrain_Relative_Linear_Velocity(const FRAME<VECTOR<T,d> >& parent_frame,VECTOR<T,d>& relative_linear_velocity) const; \
    template VECTOR<T,d> BALL_JOINT<VECTOR<T,d> >::Prismatic_Component_Translation() const; \
    template void BALL_JOINT<VECTOR<T,d> >::Constrain_Prismatically(VECTOR<T,d>& translation) const; \
    template bool BALL_JOINT<VECTOR<T,d> >::Has_Angular_Constraint() const; \
    template void BALL_JOINT<VECTOR<T,d> >::Constrain_Angles(VECTOR<T,s>& angles_input) const; \
    template void BALL_JOINT<VECTOR<T,d> >::Update_State_From_Joint_Frame(const bool enforce_constraints); \
    template void BALL_JOINT<VECTOR<T,d> >::Angular_Constraint_Matrix(const FRAME<VECTOR<T,d> >& parent_frame,const FRAME<VECTOR<T,d> >& child_frame,MATRIX_MXN<T>& constrained_matrix,MATRIX_MXN<T>* unconstrained_matrix) const; \
    template void BALL_JOINT<VECTOR<T,d> >::Initialize_Twist_Constraint(const T min_twist,const T max_twist,const FRAME<VECTOR<T,d> >& parent_frame,const FRAME<VECTOR<T,d> >& child_frame); \
    template void BALL_JOINT<VECTOR<T,d> >::Allow_Switching(const bool allow);

INSTANTIATION_HELPER(float,3,3)
template class BALL_JOINT<VECTOR<float,2> >;
template class BALL_JOINT<VECTOR<float,1> >;
INSTANTIATION_HELPER(double,3,3)
template class BALL_JOINT<VECTOR<double,2> >;
template class BALL_JOINT<VECTOR<double,1> >;
}
