/* cor_syn_ifcace.cc */

#include "cor_syn_iface.h"
#include "body_def.h"
#include "cor_link.h"
#include "element.h"
#include <coriolis/nail.h>
#include <coriolis/attachments.h>
#include "simple3d.h"
#include "id_lookup.h"
#include "function.h"
#include "root.h"
#include "shape.h"

class dc_cor_clock : public dc_clock {
  bool update( void );
public:
  dc_cor_clock( cstring );
};

dc_clock *coriolis_clock = nil; /* declared extern */

BodySystem universe; /* declared extern */

bool upheaval = false; /* declared extern */
static bool view_open = false;

static RigidBody ground;
static Shape *ground_shape = nil;

bool init_coriolis_agent( void ) {
  static bool inited = false;
  if( inited ) return true;
  inited = true;
  
  universe.dynamic = universe.friction = TRUE;
  upheaval = true;
  view_open = true;

  init_view();

  /* set up ground */
  ground.set_geometry( *( ground_shape = define_cube( 1000, 10, 1000 ) ) );
  ground.x = triple( 0, -5, 0 );
  universe.add( ground );
  ground.set_kinematic( false );
  ground.allow_rotation( false ); 
  ground.allow_translation( false );
  ground.entity_name = new char[7];
  strcpy( ground.entity_name, "ground" );
  draw_shape( *ground_shape, black_col );

  coriolis_clock = new dc_cor_clock( coriolis_clock_name );

  return false;
}

bool close_coriolis_agent( void ) {

  delete( ground.entity_name );
  ground.entity_name = nil;

  if( !view_open ) return true;
  close_view();
  view_open = false;

  delete( coriolis_clock );
  coriolis_clock = nil;

  return false;
}

double advance_coriolis_time( double max_advance ) {
  return coriolis_clock->advance( max_advance );
}

int dc_physical_link::rehash( void ) {
  src_rb = tgt_rb = nil;

  int nerrors = relink();
  if( nerrors ) return nerrors;
  
  dc_component *source = get_source(),
               *target = get_target();
  dc_shape *shape = nil;
  if( source ) {
    dc_label *id = source->lookup_child( coriolis_shape_label );
    if( id != nil ) {
      switch( id->type() ) {
      case Element_t :
	if( ( ( dc_element * )id )->get_rtype() == Shape_t ) {
	  shape = ( dc_shape * )( ( dc_element * )id )->get();
	}
	break;
      case Shape_t :
	shape = ( dc_shape * )id;
	break;
      default :;
      }
    }
  }

  if( shape == nil ) {
    dc_trace( TRACE_WARNING ) {
      cerr << "dc_physical_link::rehash -- failed to match body to source \"" 
	   << source->full_type() << "\"\n";
    }
    return 1;
  } else {
    src_rb = shape->get_rbody();
  }

  if( target ) {
    dc_label *id = target->lookup_child( coriolis_shape_label );
    if( id != nil ) {
      switch( id->type() ) {
      case Element_t :
	if( ( ( dc_element * )id )->get_rtype() == Shape_t ) {
	  shape = ( dc_shape * )( ( dc_element * )id )->get();
	}
	break;
      case Shape_t :
	shape = ( dc_shape * )id;
	break;
      default :;
      }
    }

    if( shape == nil ) {
      dc_trace( TRACE_WARNING ) {
	cerr << "dc_physical_link::rehash -- failed to match body to target\n";
      }
      return 1;
    } else {
      tgt_rb = shape->get_rbody();
    }
  }

  return 0;
}

void dc_constraint_link::add_constr( EConstraint &e ) {
  econstr_list.append( &e );
  universe.add( e );
  upheaval = true;
}

void dc_constraint_link::unset_constr( void ) {
  EConstraint *econstr;
  forall( econstr, econstr_list ) {
    if( econstr->bs != nil ) {
      econstr->bs->remove( *econstr );
    }
    delete( econstr );
  }
  econstr_list.clear();
}

dc_constraint_link::~dc_constraint_link( void ) {
  unset_constr();
}

int dc_constraint_link::rehash( void ) {
  if( src_rb != nil && tgt_rb != nil ) {
    universe.disallow_intersection( *src_rb, *tgt_rb );
  }

  int nerrors;
  if( ( nerrors = dc_physical_link::rehash() ) != 0 ) return nerrors;
  
  if( src_rb != nil && tgt_rb != nil ) {
    universe.allow_intersection( *src_rb, *tgt_rb );
  }

  return nerrors;
}

dc_distance_link::dc_distance_link( void ) {
  source_bodypt = target_bodypt = 0;
  distance = 0;
}

void dc_distance_link::set_pts( ctriple source_pt, ctriple target_pt, 
				double dist ) {
  source_bodypt = source_pt;
  target_bodypt = target_pt;
  distance = dist;
}

int dc_distance_link::rehash( void ) {
  int nerrors = dc_physical_link::rehash();
  if( nerrors ) return nerrors;

  if( src_rb == nil ) {
    return 1;
  }

  EConstraint *constr = nil;

  if( tgt_rb == nil ) { /* pin to world */
    if( distance ) {
      constr = create_dist_constraint( *src_rb, source_bodypt, 
				       target_bodypt, distance );      
    } else {
      constr = new Nail;
      ( ( Nail * )constr )->init( *src_rb, source_bodypt, target_bodypt );
      src_rb->x += target_bodypt - src_rb->T( source_bodypt );
    }
  } else {
    if( distance ) {
      constr = create_dist_constraint( *src_rb, source_bodypt, *tgt_rb, 
				       target_bodypt, distance );
    } else {
      constr = new Pinjoint;
      dc_trace( TRACE_MANY ) {      
	cerr << "MAKING NEW PINJOINT\nSOURCE_BODYPT = " << source_bodypt << "\n"
	     << "SOURCE_BP = " << source_bodypt << "\nTARGET_BP = " 
	     << target_bodypt << "\nSRC_X = "<< src_rb->x << "\nTGT_X = " 
	     << tgt_rb->x << "\nT(SRC_BP) = " << src_rb->T( source_bodypt ) 
	     << "\nT(TGT_BP) = " << tgt_rb->T( target_bodypt ) 
	     << "\nSRC_X += T(TGT_BP) - T(SRC_BP) = ";
      }
      ( ( Pinjoint * )constr )->init( *src_rb, *tgt_rb, source_bodypt, 
				      target_bodypt );
      src_rb->x += tgt_rb->T( target_bodypt ) - src_rb->T( source_bodypt );
      dc_trace( TRACE_MANY ) {
	cerr << src_rb->x << "\n\n";
      }
    }
  }
  unset_constr();
  if( constr )
    add_constr( *constr );

  return 0;
}

bool dc_distance_link::exec( void ) {
  if( view_open ) {
    triple src_pt = source_bodypt;
    triple tgt_pt = target_bodypt; 
    if( src_rb ) src_pt = src_rb->T( src_pt );
    if( tgt_rb ) tgt_pt = tgt_rb->T( tgt_pt );
    draw_link( src_pt, tgt_pt, get_directed(), red_col );
  }
  return false;
}

ostream &dc_distance_link::display( ostream &stream = cout ) const {
  dc_link::display( stream );
  return stream << source_bodypt << ", " << target_bodypt << ", " << distance;
}

dc_axial_link::dc_axial_link( void ) {
  /* assign arbitrary unit axis to source/target_ptb */
  source_ptb = target_ptb = triple( 1, 0, 0 ); 
}

bool dc_axial_link::set_pts( ctriple sa, ctriple sb, ctriple ta, ctriple tb ) {
  if( sa != sb && ta != tb ) {
    source_pta = sa;
    source_ptb = sb;
    target_pta = ta;
    target_ptb = tb;
    return false;
  }
  dc_trace( TRACE_ERROR ) {
    cerr << "dc_axial_link::set_pts -- illegal link points ( " 
	 << sa << ", " << sb << " ), ( " << ta << ", " << tb << " )\n";
  }
  return true;
}

int dc_axial_link::rehash( void ) {
  int nerrors = dc_physical_link::rehash();
  if( nerrors ) return nerrors;

  if( src_rb == nil ) {
    return 1;
  }

  EConstraint *constr1 = nil, *constr2 = nil;

  if( tgt_rb == nil ) { /* constrain to world */
    create_axial_link( *src_rb, source_pta, source_ptb, target_pta, target_ptb, 
		       constr1, constr2 );
    /* move src_pta onto tgt_pta */
    src_rb->x += target_pta - src_rb->T( source_pta );    
  } else {
    create_axial_link( *src_rb, source_pta, source_ptb, 
		       *tgt_rb, target_pta, target_ptb, constr1, constr2 );
    /* move src_pta onto tgt_pta */
    src_rb->x += tgt_rb->T( target_pta ) - src_rb->T( source_pta );
  }
  
  unset_constr();
  if( constr2 ) {
    add_constr( *constr1 );
    add_constr( *constr2 );
  }
  return ( constr2 == nil );
}

bool dc_axial_link::exec( void ) {
  if( view_open ) {
    if( src_rb ) {
      source_pta = src_rb->T( source_pta );
      source_ptb = src_rb->T( source_ptb );
    }
    if( tgt_rb ) {
      target_pta = tgt_rb->T( target_pta );
      target_ptb = tgt_rb->T( target_ptb );
    }
    cerr << "drawing link ( " << source_pta << ", " << source_ptb << " ), ( "
	 << target_pta << ", " << target_ptb << " )\n";
    draw_link( source_pta, target_pta, get_directed(), blue_col );
    draw_link( source_ptb, target_ptb, get_directed(), blue_col );
  }
  return false;
}

ostream &dc_axial_link::display( ostream &stream = cout ) const {
  dc_link::display( stream );
  return stream << source_pta << ", " << source_ptb << ", " 
		<< target_pta << ", " << target_ptb;
}

int dc_attachment_link::rehash( void ) {
  int nerrors = dc_constraint_link::rehash();
  if( nerrors ) return nerrors;

  if( !src_rb || !tgt_rb ) return 1;
  
  EConstraint *constr1 = nil, *constr2 = nil, *constr3 = nil;

  create_attachment( *src_rb, *tgt_rb, constr1, constr2, constr3 );
    
  unset_constr();
  if( constr3 ) {
    add_constr( *constr1 );
    add_constr( *constr2 );
    add_constr( *constr3 );
  }
  return ( constr3 == nil );
}

dc_variable_force::~dc_variable_force( void ) {
  if( force ) delete( force ); 
  if( src_bp ) delete( src_bp );
  if( tgt_bp ) delete( tgt_bp );
}

void dc_variable_force::init( dc_func &f, dc_func &s, dc_func &t ) {
  if( force ) delete( force );
  if( src_bp ) delete( src_bp );
  if( tgt_bp ) delete( tgt_bp );

  force = &f;
  src_bp = &s;
  tgt_bp = &t;
}

void dc_variable_force::init( dc_func &f, dc_func &s ) {
  if( force ) delete( force );
  if( src_bp ) delete( src_bp );
  if( tgt_bp ) delete( tgt_bp );

  force = &f;
  src_bp = &s;
  tgt_bp = nil;
}

void dc_variable_force::update( void ) {
  //cerr << "IN FORCE UPDATE\n";
  dc_triple *F = ( dc_triple * )force->evaluate();
  if( F == nil ) return;

  //  cerr << "FORCE = " << *F << "\n";

  triple t( (*F)(1), (*F)(2), (*F)(3) );

  t = t * ( bs->current_time - bs->last_time );
  //  cerr << "bs->t = " << bs->current_time << ", old_t = " << bs->last_time << "\n";
  //  cerr << "FORCE TIME NORMALZIED = " << t << "\n";

  if( src && src_bp ) {
    //    cerr << "SOURCE = \"" << src->entity_name << ", mass = " << src->mass << "\"\n";
    dc_triple *S = ( dc_triple * )src_bp->evaluate();
    triple pt( (*S)(1), (*S)(2), (*S)(3) );
    //    cerr << "SRC BP = " << *S << "\tFORCE PT = " << pt << "\n";
    
    exert_force( *src, src->point_force( pt, t ) );
    delete( S );
  }
  if( tgt && tgt_bp ) {
    //    cerr << "TARGET = \"" << tgt->entity_name << "\"\n";
    dc_triple *T = ( dc_triple * )tgt_bp->evaluate();
    triple pt( (*T)(1), (*T)(2), (*T)(3) );
    //    cerr << "TGT BP = " << *T << "\tFORCE PT = " << pt << "\n";

    exert_force( *tgt, tgt->point_force( pt, -t ) );
    delete( T );
  }

  delete( F );
}

void dc_variable_force::ignore( Body &b ) { 
  if( src == &b ) src = nil;
  if( tgt == &b ) tgt = nil;
}

int dc_variable_force::rehash( void ) {
  int nerrors = 0;
  list<tag> dep_list;
  dc_type T;

  if( force ) {
    nerrors += force->rehash( dep_list, tag_error );
    if( ( T = force->get_rtype() ) != Triple_t ) {
      dc_trace( TRACE_ERROR ) {
	cerr << "dc_variable_force -- type of force ( " << *force 
	     << " ) must be a triple. Was " << dc_type_string[T] << "\n";
      }
      nerrors++;
    }
  }

  if( src_bp ) { 
    nerrors += src_bp->rehash( dep_list, tag_error );
    if( ( T = src_bp->get_rtype() ) != Triple_t ) {
      dc_trace( TRACE_ERROR ) {
	cerr << "dc_variable_force -- type of source pt ( " << *src_bp 
	     <<  " ) must be a triple. Was " << dc_type_string[T] << "\n";
      }
      nerrors++;
    }
  }

  if( tgt_bp ) {
    nerrors += tgt_bp->rehash( dep_list, tag_error );
    if( ( T = tgt_bp->get_rtype() ) != Triple_t ) {
      dc_trace( TRACE_ERROR ) {
	cerr << "dc_variable_force -- type of target pt ( " << *tgt_bp 
	     << " ) must be a triple. Was " << dc_type_string[T] << "\n";
      }
      nerrors++;
    }
  }
  
  return nerrors;
}

dc_force_link::dc_force_link( void ) {
  universe.add( I );
}

dc_force_link::~dc_force_link( void ) {
  universe.remove( I );
}

bool dc_force_link::set_pts( dc_func &f, dc_func &s, dc_func &t ) {
  /* if( f.get_rtype() != Triple_t ) return true;
  if( s.get_rtype() != Triple_t ) return true;
  if( t.get_rtype() != Triple_t ) return true; */

  I.init( f, s, t );
  return false;
}

bool dc_force_link::set_pts( dc_func &f, dc_func &s ) {
  /*   if( f.get_rtype() != Triple_t ) return true;
       if( s.get_rtype() != Triple_t ) return true; */

  I.init( f, s );
  return false;
}

int dc_force_link::rehash( void ) {
  int nerrors = dc_physical_link::rehash();
  if( nerrors ) return nerrors;

  universe.remove( I );
  
  I.src = src_rb;
  I.tgt = tgt_rb;

  nerrors += I.rehash();

  universe.add( I );

  upheaval = true;
  return nerrors;
}

ostream &dc_force_link::display( ostream &stream = cout ) const {
  dc_link::display( stream );
  if( I.force ) 
    stream << *( I.force );
  else stream << "<nil>";
  
  if( I.src_bp ) {
    stream << ", " << *(I.src_bp);
  } else {
    stream << "<nil>";
  }
  if( I.tgt_bp ) 
    stream << ", " << *(I.tgt_bp);
  return stream << "\n";
}

dc_cor_clock::dc_cor_clock( cstring label ) {
  set_label( label );
}

void draw_dcshape( dc_shape_core &s, void * ) {
  s.draw();
}

bool dc_cor_clock::update( void ) {
  if( upheaval ) {
    universe.upheaval();
    //    cerr << "UPHEAVAL\n";
    upheaval = false;
  }

  double dt;
  set_interval( dt = universe.step_forward( get_initial_interval() ) );
  //  cerr << "STEPPED " << dt << "\n";

  clear_view();
  draw_shape( *ground_shape, black_col );
  forall_shapes( draw_dcshape );

  return false;
}
