//#####################################################################
// Copyright 2004-2007, Craig Schroeder, Tamar Shinar, Eftychios Sifakis, 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/Articulated_Rigid_Bodies/ARTICULATED_RIGID_BODY_2D.h>
#include <Core/Implicit_Objects/IMPLICIT_OBJECT.h>
#include <Core/Matrices/MATRIX_MXN.h>
#include <Core/Meshes/SEGMENTED_CURVE.h>
#include <Core/Read_Write/FILE_UTILITIES.h>
#include <Core/Utilities/VIEWER_DIR.h>
#include <Rigids/Joints/JOINT.h>
#include <Rigids/Joints/JOINT_FUNCTION.h>
#include <Rigids/Joints/JOINT_MESH.h>
#include <Rigids/Rigid_Bodies/RIGID_BODY.h>
#include <Rigids/Rigid_Bodies/RIGID_BODY_MASS.h>
using namespace PhysBAM;
//#####################################################################
// Constructor
//#####################################################################
template<class T> ARTICULATED_RIGID_BODY<VECTOR<T,2> >::
ARTICULATED_RIGID_BODY(RIGID_BODY_COLLECTION<TV>& rigid_body_collection_input)
    :BASE(rigid_body_collection_input)
{}
//#####################################################################
// Destructor
//#####################################################################
template<class T> ARTICULATED_RIGID_BODY<VECTOR<T,2> >::
~ARTICULATED_RIGID_BODY()
{}
//####################################################################################
// Compute_Position_Based_State
//####################################################################################
template<class T> void ARTICULATED_RIGID_BODY<VECTOR<T,2> >::
Compute_Position_Based_State(const T dt,const T time)
{
    for(int i=0;i<joint_mesh.Num_Joints();i++) if(joint_mesh.Joints(i)->joint_function) joint_mesh.Joints(i)->joint_function->Compute_Desired_PD_Velocity(dt,time);
}
//####################################################################################
// Compute_Target_PD_Angular_Impulse
//####################################################################################
template<class T> VECTOR<T,1> ARTICULATED_RIGID_BODY<VECTOR<T,2> >::
Compute_Target_PD_Angular_Impulse(const JOINT_ID joint_id)
{
    JOINT<TV>& joint=*joint_mesh(joint_id);PHYSBAM_ASSERT(joint.joint_function);
    RIGID_BODY<TV> &parent=*Parent(joint_id),&child=*Child(joint_id);
    if(parent.Has_Infinite_Inertia() && child.Has_Infinite_Inertia()) return VECTOR<T,1>((T)0);
    parent.Update_Angular_Velocity();child.Update_Angular_Velocity();
    // IMPORTANT: joint function's relative velocity is child w.r.t. parent while rigid body uses parent w.r.t. child
    VECTOR<T,1> relative_angular_velocity=RIGID_BODY<TV>::Relative_Angular_Velocity(parent,child),
        relative_angular_velocity_new=-joint.joint_function->desired_angular_velocity;
    MATRIX<T,1> I1_inv_plus_I2_inv;if(!parent.Has_Infinite_Inertia()) I1_inv_plus_I2_inv+=parent.Inertia_Tensor().Inverse();
    if(!child.Has_Infinite_Inertia()) I1_inv_plus_I2_inv+=child.Inertia_Tensor().Inverse();
    return (I1_inv_plus_I2_inv.Inverse())*(relative_angular_velocity_new-relative_angular_velocity);
}
//####################################################################################
// Post_Stabilization_With_Actuation
//####################################################################################
template<class T> void ARTICULATED_RIGID_BODY<VECTOR<T,2> >::
Post_Stabilization_With_Actuation(const JOINT_ID joint_id)
{
    JOINT<TV>& joint=*joint_mesh(joint_id);
    if(joint.joint_function && joint.joint_function->active){
        RIGID_BODY<TV> &parent=*Parent(joint_id),&child=*Child(joint_id);
        if(parent.Has_Infinite_Inertia() && child.Has_Infinite_Inertia()) return;

        VECTOR<T,1> angular_impulse=Compute_Target_PD_Angular_Impulse(joint_id);
        if(use_pd_actuators) RIGID_BODY<TV>::Apply_Impulse(parent,child,TV(),TV(),angular_impulse);}

    Apply_Poststabilization_To_Joint(joint_id);
}
//####################################################################################
// Solve_Velocities_for_PD
//####################################################################################
template<class T> void ARTICULATED_RIGID_BODY<VECTOR<T,2> >::
Solve_Velocities_for_PD(const T time,const T dt,bool test_system,bool print_matrix)
{
    // don't do extra actuation iterations if not using actuators
    if(!use_pd_actuators){Apply_Poststabilization(test_system,print_matrix);return;}
    for(int iteration=0;iteration<actuation_iterations;iteration++) for(int k=0;k<poststabilization_iterations;k++)
        for(int i=0;i<joint_mesh.Num_Joints();i++) Post_Stabilization_With_Actuation(joint_mesh.Joints(i)->id_number);
}
//####################################################################################
// Function Create_Joint_Function
//####################################################################################
template<class T> JOINT_FUNCTION<VECTOR<T,2> >* ARTICULATED_RIGID_BODY<VECTOR<T,2> >::
Create_Joint_Function(const JOINT_ID joint_id)
{
    joint_mesh(joint_id)->Set_Joint_Function(new JOINT_FUNCTION<TV>(*joint_mesh(joint_id),*Parent(joint_id),*Child(joint_id)));
    return joint_mesh(joint_id)->joint_function;
}
//####################################################################################
// Output_Articulation_Points
//####################################################################################
template<class T> void ARTICULATED_RIGID_BODY<VECTOR<T,2> >::
Output_Articulation_Points(const STREAM_TYPE stream_type,const VIEWER_DIR& viewer_dir) const
{
    if(joint_mesh.Num_Joints()==0) return;
    FILE_OSTREAM output(stream_type,viewer_dir.Frame_File("arb_info"));
    Write_Binary(output,joint_mesh.Num_Joints()*2);
    for(int i=0;i<joint_mesh.Num_Joints();i++){
        JOINT<TV>& joint=*joint_mesh.Joints(i);
        const RIGID_BODY<TV> &parent=*Parent(joint.id_number),&child=*Child(joint.id_number);
        TV ap1=parent.World_Space_Point(joint.F_pj().t),ap2=child.World_Space_Point(joint.F_cj().t);
        Write_Binary(output,ap1,ap2);}
}
//#####################################################################
namespace PhysBAM{
template class ARTICULATED_RIGID_BODY<VECTOR<float,2> >;
template class ARTICULATED_RIGID_BODY<VECTOR<double,2> >;
}
