/* pipe_iface.cc */

#include "pipe_iface.h"
#include "component.h"
#include "cor_pipe.h"
#include "data_classes.h"
#include "units.h"
#include "shape.h"
#include "clock.h"
#include "cor_syn_iface.h"
#include "root.h"

dc_pipe *alloc_pipe( const string &label, dc_component *parent ) {
  for( int i = 1 ; i < NUM_CPT ; i++ ) {
    if( label == pipe_table[i].name ) {
      if( !pipe_table[i].valid( parent ) ) return nil;
      dc_pipe *P;
      switch( i ) {
      case mass_cpt : case position_cpt : case orientation_cpt :
      case force_cpt : case torque_cpt : case vel_cpt : case omega_cpt :
	P = new dc_cor_pipe( ( cpipe_type )i, label, parent );
	coriolis_clock->add( *P );
	break;
      default : 
	P = new dc_pipe( ( cpipe_type )i, label, parent );
	root.iter_counter->add( *P );
      }
      if( P->get_pipe_type() == undef_cpt ) {
	cerr << "alloc_pipe -- error allocating pipe of type \"" 
	     << label << "\"\n";
	delete( P ); 
	return nil;
      }
      return P;
    }
  }
  return nil;
}

/* VALIDITY FNS */
bool is_physical_pfn( dc_component *parent ) {
  if( parent == nil ) return false;
  dc_label *id = parent->lookup_child( coriolis_shape_label );
  if( !id ) return false;
  switch( id->type() ) {
  case Element_t :
    return ( ( ( dc_element * )id )->get_rtype() ) == Shape_t;
  case Shape_t : return true;
  default : return false;
  }
}

/* PIPE SET FNS */
bool cor_set_mass( dc_pipe &P, dc_data &n ) {
  dc_component *parent = ( dc_component * )P.get_parent();
  if( parent == nil ) return true;

  RigidBody *rb = ( ( dc_cor_pipe * )&P )->get_rbody();
  if( !rb ) return true;
  switch( n.type() ) {
  case Int_t :
    rb->mass = ( ( dc_int * )&n )->get();
    break;
  case Real_t :
    rb->mass = ( ( dc_real * )&n )->get();
    break;
  default : return true;
  }
  upheaval = true;
  return false;
}

bool cor_set_X( dc_pipe &P, dc_data &n ) {
  //  cerr << "SETTING " << P.full_type() << " to " << n << "\n";
  dc_component *parent = ( dc_component * )P.get_parent();
  if( parent == nil ) return true;

  RigidBody *rb = ( ( dc_cor_pipe * )&P )->get_rbody();
  if( !rb ) return true;
  
  switch( n.sub_type() ) {
  case Triple_t :
    rb->x = triple( ( *( ( dc_triple * )&n ) )(1),
		    ( *( ( dc_triple * )&n ) )(2),
		    ( *( ( dc_triple * )&n ) )(3) );
    break;
  case Vector_t :
    if( ( ( dc_vector * )&n )->dim() != 3 ) return true;
    rb->x = triple( ( *( ( dc_vector * )&n ) )(1),
		    ( *( ( dc_vector * )&n ) )(2),
		    ( *( ( dc_vector * )&n ) )(3) );
    break;
  default : return true;
  }
  upheaval = true;
  return false;
}

bool cor_set_Q( dc_pipe &P, dc_data &n ) {
  dc_component *parent = ( dc_component * )P.get_parent();
  if( parent == nil ) return true;

  RigidBody *rb = ( ( dc_cor_pipe * )&P )->get_rbody();
  if( !rb ) return true;
  switch( n.sub_type() ) {
  case Vector_t :
    if( ( ( dc_vector * )&n )->dim() != 4 ) return true;
    rb->Q = quaternion( ( *( ( dc_vector * )&n ) )(1), 
			( *( ( dc_vector * )&n ) )(2), 
			( *( ( dc_vector * )&n ) )(3), 
			( *( ( dc_vector * )&n ) )(4) );
    break;
  default : return true;
  }
  upheaval = true;
  return false;
}

bool cor_set_F( dc_pipe &/*P*/, dc_data &/*n*/ ) {
  //   dc_component *parent = ( dc_component * )P.get_parent();
  //   if( parent == nil ) return true;
  
  //   RigidBody *rb = ( ( dc_cor_pipe * )&P )->get_rbody();
  // if( !rb ) return true;
  return true;
}

bool cor_set_T( dc_pipe &/*P*/, dc_data &/*n*/ ) {
  //   dc_component *parent = ( dc_component * )P.get_parent();
  //   if( parent == nil ) return true;
  
  //   RigidBody *rb = ( ( dc_cor_pipe * )&P )->get_rbody();
  // if( !rb ) return true;
  return true;
}

bool cor_set_V( dc_pipe &P, dc_data &n ) {
  dc_component *parent = ( dc_component * )P.get_parent();
  if( parent == nil ) return true;

  RigidBody *rb = ( ( dc_cor_pipe * )&P )->get_rbody();
  if( !rb ) return true;
  switch( n.sub_type() ) {
  case Triple_t :
    rb->v = triple( ( *( ( dc_vector * )&n ) )(1),
		    ( *( ( dc_vector * )&n ) )(2),
		    ( *( ( dc_vector * )&n ) )(3) );
    break;
  case Vector_t :
    if( ( ( dc_vector * )&n )->dim() != 3 ) return true;
    rb->v = triple( ( *( ( dc_vector * )&n ) )(1), 
		    ( *( ( dc_vector * )&n ) )(2), 
		    ( *( ( dc_vector * )&n ) )(3) );
    break;
  default : return true;
  }
  upheaval = true;
  return false;
}

bool cor_set_U( dc_pipe &P, dc_data &n ) {
  dc_component *parent = ( dc_component * )P.get_parent();
  if( parent == nil ) return true;

  RigidBody *rb = ( ( dc_cor_pipe * )&P )->get_rbody();
  if( !rb ) return true;
  switch( n.sub_type() ) {
  case Triple_t :
    rb->omega = triple( ( *( ( dc_vector * )&n ) )(1),
			( *( ( dc_vector * )&n ) )(2),
			( *( ( dc_vector * )&n ) )(3) );
    break;
  case Vector_t :
    if( ( ( dc_vector * )&n )->dim() != 3 ) return true;
    rb->omega = triple( ( *( ( dc_vector * )&n ) )(1), 
			( *( ( dc_vector * )&n ) )(2), 
			( *( ( dc_vector * )&n ) )(3) );
    break;
  default : return true;
  }
  upheaval = true;
  return false;
}

/* PIPE GET FNS */
dc_data *cor_get_mass( dc_pipe &P ) {
  dc_component *parent = ( dc_component * )P.get_parent();
  if( parent == nil ) return nil;

  RigidBody *rb = ( ( dc_cor_pipe * )&P )->get_rbody();
  if( !rb ) return nil;  
  dc_data *n = new dc_real( rb->mass );
  unit_vec *uv = &( ( ( dc_udata * )n )->get_units() );
  ( *uv )[uMass] = 1;
  return n;
}

dc_data *cor_get_X( dc_pipe &P ) {
  dc_component *parent = ( dc_component * )P.get_parent();
  if( parent == nil ) return nil;

  RigidBody *rb = ( ( dc_cor_pipe * )&P )->get_rbody();
  if( !rb ) return nil;  
  triple tr =  rb->T( ( ( dc_cor_pipe * )&P )->get_loc() );
  dc_data *n = new dc_triple( tr(0), tr(1), tr(2) );
  unit_vec *uv = &( ( ( dc_udata * )n )->get_units() );
  ( *uv )[uDisp] = 1;
  return n;
}

dc_data *cor_get_Q( dc_pipe &P ) {
  dc_component *parent = ( dc_component * )P.get_parent();
  if( parent == nil ) return nil;

  RigidBody *rb = ( ( dc_cor_pipe * )&P )->get_rbody();
  if( !rb ) return nil;  
  dc_data *n = new dc_vector( 4 );
  double *d = ( ( dc_vector * )n )->get_store();
  d[0] = rb->Q.r;
  d[1] = rb->Q.i;
  d[2] = rb->Q.j;
  d[3] = rb->Q.k;
  unit_vec *uv = &( ( ( dc_vector * )n )->get_units() );
  ( *uv )[uAngle] = 1;
  return n;
}

dc_data *cor_get_F( dc_pipe &/*P*/ ) {
  //  dc_component *parent = ( dc_component * )P.get_parent();
  //  if( parent == nil ) return nil;

  //  RigidBody *rb = ( ( dc_cor_pipe * )&P )->get_rbody();
  //  if( !rb ) return nil;  
  return nil;
  //  return n;
}

dc_data *cor_get_T( dc_pipe &/*P*/ ) {
  //  dc_component *parent = ( dc_component * )P.get_parent();
  //  if( parent == nil ) return nil;

  //  RigidBody *rb = ( ( dc_cor_pipe * )&P )->get_rbody();
  //  if( !rb ) return nil;  
  return nil;
  //  return n;
}

dc_data *cor_get_V( dc_pipe &P ) {
  dc_component *parent = ( dc_component * )P.get_parent();
  if( parent == nil ) return nil;

  RigidBody *rb = ( ( dc_cor_pipe * )&P )->get_rbody();
  if( !rb ) return nil;  
  triple tr =  rb->Tdot( ( ( dc_cor_pipe * )&P )->get_loc() );
  dc_data *n = new dc_triple( tr(0), tr(1), tr(2) );
  unit_vec *uv = &( ( ( dc_udata * )n )->get_units() );
  ( *uv )[uDisp] = 1;
  ( *uv )[uTime] = -1;
  return n;
}

dc_data *cor_get_U( dc_pipe &P ) {
  dc_component *parent = ( dc_component * )P.get_parent();
  if( parent == nil ) return nil;

  RigidBody *rb = ( ( dc_cor_pipe * )&P )->get_rbody();
  if( !rb ) return nil;  
  triple tr =  rb->omega;
  dc_data *n = new dc_triple( tr(0), tr(1), tr(2) );
  unit_vec *uv = &( ( ( dc_udata * )n )->get_units() );
  ( *uv )[uAngle] = 1;
  ( *uv )[uTime] = -1;
  return n;
}
