/*****************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "communicate.h"

/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/* Defines */

/* Return values: */
#define OK 1
#define ERROR -1 

/* Generic good stuff */
#define TRUE 1
#define FALSE 0

#define SCREEN_UPDATE_PERIOD 0.030

/*****************************************************************************/

Communication_Buffer *tcb = NULL;

/*****************************************************************************/
/*****************************************************************************/

char *default_iv_filename( Communication_Buffer *cb )
{
  return "blocks-world.iv";
}

/*****************************************************************************/

float
default_time_step( Communication_Buffer *cb )
{
  return SCREEN_UPDATE_PERIOD;
}

/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/

void
init_simulation( Communication_Buffer *cb )
{
  pSimulator init_simulator();

  printf( "init_simulation...\n" );
  tcb = cb;
  cb->playing = FALSE;
  cb->time = 0.0;

  cb->ps = init_simulator();
}

/*****************************************************************************/

int index = 0;

void
set_desireds( Communication_Buffer *cb )
{

  if ( !cb->playing )
    printf( "\n%d\n", index );

  index++;
}

/*****************************************************************************/
/*****************************************************************************/

advance_one_step( Communication_Buffer *cb )
{
  simulate( cb->ps );
}

/*****************************************************************************/

int count = 0;

void
simulate_up_to_time( float stop_time, float mouse_x, float mouse_y,
		    Communication_Buffer *cb )
{

  if ( !cb->playing )
    return;

  if ( count % 100 == 0 )
    printf( "%d: %f %f %f\n", count, stop_time, cb->ps->time,
	    stop_time - cb->ps->time );
  count++;

  advance_one_step( cb );
}

/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/

void
dump_state( Communication_Buffer *cb )
{
  int i;
}

/*****************************************************************************/

void
save_data( Communication_Buffer *cb )
{
  printf( "save_data() not implemented.\n" );
}

/*****************************************************************************/

void
handle_goto( Communication_Buffer *cb )
{
  int i;

  printf( "handle_goto()\n" );
}

/*****************************************************************************/

void
handle_step( Communication_Buffer *cb )
{
  if ( !cb->playing )
    {
      advance_one_step( cb );
    }
  else
    cb->playing = FALSE;
}

/*****************************************************************************/

void
handle_play( Communication_Buffer *cb )
{
  cb->playing = TRUE;
}

/*****************************************************************************/

void
handle_mouse_left( float mouse_x, float mouse_y,
		    Communication_Buffer *cb )
{
  handle_step( cb );
}

/*****************************************************************************/

void
handle_mouse_middle( float mouse_x, float mouse_y,
		    Communication_Buffer *cb )
{
  if ( cb->playing )
    handle_step( cb );
  else
    handle_play( cb );
}

/*****************************************************************************/

void
handle_mouse_right( float mouse_x, float mouse_y,
		    Communication_Buffer *cb )
{
  handle_play( cb );
}

/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/

/*****************************************************************************/
/*****************************************************************************/

pSimulator
init_simulator()
{
  int i, index;
  pSimulator ps;

  ps = (pSimulator) malloc( sizeof( Simulator ) );
  if ( ps == NULL )
    {
      fprintf( stderr, "Failed to allocate simulator in init_simulator()\n" );
      exit( -1 );
    }

  /* Initialize simulator structure. */
  ps->time = 0.0;
  ps->time_step = 0.03;

  /* Start with no arms */
  for( i = 0; i < MAX_N_ARMS; i++ )
    ps->arms[i] = NULL;

  /* Start with no blocks */
  for( i = 0; i < MAX_N_BLOCKS; i++ )
    ps->blocks[i] = NULL;

  /* Add an arm */
  if ( MAX_N_ARMS > 0 )
    {
      ps->arms[0] = (pArm) malloc( sizeof( Arm ) );
      if ( ps->arms[0] == NULL )
	{
	  fprintf( stderr, "Failed to allocate arm in init_simulator()\n" );
	  exit( -1 );
	}
      ps->arms[0]->link1 = 0.4;
      ps->arms[0]->link2 = 0.4;
      ps->arms[0]->link3 = 0.1;
      ps->arms[0]->finger = 0.05;
      ps->arms[0]->base = 0;
      ps->arms[0]->shoulder = 0;
      ps->arms[0]->elbow = 0;
      ps->arms[0]->wrist = 0;
      ps->arms[0]->gripper = 0.1;
    }

  /* Add a block */
  if ( MAX_N_BLOCKS > 0 )
    {
      index = 0;
      ps->blocks[index] = (pBlock) malloc( sizeof( Block ) );
      if ( ps->blocks[index] == NULL )
	{
	  fprintf( stderr, "Failed to allocate block in init_simulator()\n" );
	  exit( -1 );
	}
      ps->blocks[index]->width = 0.1;
      ps->blocks[index]->height = 0.2;
      ps->blocks[index]->x = 1.0;
      ps->blocks[index]->y = 0.0;
      ps->blocks[index]->angle = 0;
    }

  /* Add a block */
  if ( MAX_N_BLOCKS > 1 )
    {
      index = 1;
      ps->blocks[index] = (pBlock) malloc( sizeof( Block ) );
      if ( ps->blocks[index] == NULL )
	{
	  fprintf( stderr, "Failed to allocate block in init_simulator()\n" );
	  exit( -1 );
	}
      ps->blocks[index]->width = 0.1;
      ps->blocks[index]->height = 0.1;
      ps->blocks[index]->x = 0.7;
      ps->blocks[index]->y = 0.0;
      ps->blocks[index]->angle = 0;
    }

  /* Add a block */
  if ( MAX_N_BLOCKS > 2 )
    {
      index = 2;
      ps->blocks[index] = (pBlock) malloc( sizeof( Block ) );
      if ( ps->blocks[index] == NULL )
	{
	  fprintf( stderr, "Failed to allocate block in init_simulator()\n" );
	  exit( -1 );
	}
      ps->blocks[index]->width = 0.1;
      ps->blocks[index]->height = 0.1;
      ps->blocks[index]->x = -1.0;
      ps->blocks[index]->y = 0.0;
      ps->blocks[index]->angle = 0;
    }

  for( i = 0; i < MAX_N_ARMS; i++ )
    {
      arm_forward_kinematics( i, ps );
    }

  for( i = 0; i < MAX_N_BLOCKS; i++ )
    {
      block_forward_kinematics( i, ps );
    }

  return ps;
}

/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/

/* Simulate the cart-pole for one time step. */
simulate( ps )
     register pSimulator ps;
{
  int i;
  int return_value;
  float screen_width;

  your_planner_goes_here( ps );
  dynamics( ps );

  /* Check for CRASH goes here */
  return_value = OK;

  return( return_value );
}

/*****************************************************************************/
/*****************************************************************************/

your_planner_goes_here( ps )
     register pSimulator ps;
{
  /* simulate sensing here. */
  /* then do planning */
  /* then generate robot commands (in this case the velocities) */

  /* A cheap example. */
  ps->arms[0]->base_velocity = cos( (double) (ps->time) );
  ps->arms[0]->shoulder_velocity = cos( (double) (ps->time) );
  ps->arms[0]->elbow_velocity = -2*cos( (double) (ps->time) );
  ps->arms[0]->wrist_velocity = cos( (double) (ps->time) );
  ps->arms[0]->gripper_velocity = 0.1*cos( (double) (ps->time) );
}

/*****************************************************************************/
/*****************************************************************************/

dynamics( ps )
     register pSimulator ps;
{
  int i;

  ps->time += ps->time_step;

  for( i = 0; i < MAX_N_ARMS; i++ )
    {
      arm_forward_dynamics( i, ps );
      arm_forward_kinematics( i, ps );
    }

  for( i = 0; i < MAX_N_BLOCKS; i++ )
    {
      block_forward_kinematics( i, ps );
    }
}

/*****************************************************************************/
/*****************************************************************************/

arm_forward_dynamics( index, ps )
     int index;
     register pSimulator ps;
{
  register pArm pa;

  pa = ps->arms[index];

  /* Check if there is an arm there */
  if ( pa == NULL )
    return;

  pa->base += pa->base_velocity*ps->time_step;
  pa->shoulder += pa->shoulder_velocity*ps->time_step;
  pa->elbow += pa->elbow_velocity*ps->time_step;
  pa->wrist += pa->wrist_velocity*ps->time_step;
  pa->gripper += pa->gripper_velocity*ps->time_step;
}

/*****************************************************************************/

arm_forward_kinematics( index, ps )
     int index;
     register pSimulator ps;
{
  register pArm pa;

  pa = ps->arms[index];

  /* Check if there is an arm there */
  if ( pa == NULL )
    return;

  pa->c_shoulder = cos( (double) (pa->shoulder) );
  pa->s_shoulder = sin( (double) (pa->shoulder) );
  pa->c_elbow = cos( (double) (pa->shoulder + pa->elbow) );
  pa->s_elbow = sin( (double) (pa->shoulder + pa->elbow) );
  pa->c_wrist = cos( (double) (pa->shoulder + pa->elbow + pa->wrist) );
  pa->s_wrist = sin( (double) (pa->shoulder + pa->elbow + pa->wrist) );

  pa->base_x = pa->base;
  pa->base_y = 1.0;
  pa->elbow_x = pa->base_x + pa->link1*pa->s_shoulder;
  pa->elbow_y = pa->base_y - pa->link1*pa->c_shoulder;
  pa->wrist_x = pa->elbow_x + pa->link2*pa->s_elbow;
  pa->wrist_y = pa->elbow_y - pa->link2*pa->c_elbow;
  pa->gripper_x = pa->wrist_x + pa->link3*pa->s_wrist;
  pa->gripper_y = pa->wrist_y - pa->link3*pa->c_wrist;
  pa->lgripper_x = pa->gripper_x - 0.5*pa->gripper*pa->c_wrist;
  pa->lgripper_y = pa->gripper_y - 0.5*pa->gripper*pa->s_wrist;
  pa->rgripper_x = pa->gripper_x + 0.5*pa->gripper*pa->c_wrist;
  pa->rgripper_y = pa->gripper_y + 0.5*pa->gripper*pa->s_wrist;
  pa->ltip_x = pa->lgripper_x + pa->finger*pa->s_wrist;
  pa->ltip_y = pa->lgripper_y - pa->finger*pa->c_wrist;
  pa->rtip_x = pa->rgripper_x + pa->finger*pa->s_wrist;
  pa->rtip_y = pa->rgripper_y - pa->finger*pa->c_wrist;
}

/*****************************************************************************/
/*****************************************************************************/

block_forward_kinematics( index, ps )
     int index;
     register pSimulator ps;
{
  register pBlock pb;

  pb = ps->blocks[index];

  /* Check if there is a block there */
  if ( pb == NULL )
    return;

  pb->c_angle = cos( (double) (pb->angle) );
  pb->s_angle = sin( (double) (pb->angle) );

  pb->c2_x = pb->x + pb->width*pb->c_angle;
  pb->c2_y = pb->y + pb->width*pb->s_angle;
  pb->c3_x = pb->c2_x - pb->height*pb->s_angle;
  pb->c3_y = pb->c2_y + pb->height*pb->c_angle;
  pb->c4_x = pb->x - pb->height*pb->s_angle;
  pb->c4_y = pb->y + pb->height*pb->c_angle;
}

/*****************************************************************************/
/*****************************************************************************/