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

/* How many things can we have in the simulator? */
#define MAX_N_ARMS 1
#define MAX_N_BLOCKS 10

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

typedef struct arm
{
  /* Link lengths in meters */
  float link1;
  float link2;
  float link3;
  float finger;

  /* Joint values */
  float base;                 /* meters, zero is middle */
  float shoulder;             /* radians, zero is straight down. */
  float elbow;                /* radians, zero is straight down. */
  float wrist;                /* radians, zero is straight down. */
  float gripper;              /* meters, zero is all the way down */

  /* Joint velocities */
  float base_velocity;
  float shoulder_velocity;
  float elbow_velocity;
  float wrist_velocity;
  float gripper_velocity;

  /* Stuff calculated by forward kinematics */
  float base_x;
  float base_y;
  float elbow_x;
  float elbow_y;
  float wrist_x;
  float wrist_y;
  float gripper_x;
  float gripper_y;
  float lgripper_x;
  float lgripper_y;
  float rgripper_x;
  float rgripper_y;
  float ltip_x;
  float ltip_y;
  float rtip_x;
  float rtip_y;
  float c_shoulder;
  float s_shoulder;
  float c_elbow;
  float s_elbow;
  float c_wrist;
  float s_wrist;

} Arm, *pArm;

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

typedef struct block
{
  /* size */
  float width;
  float height;

  /* position */
  float x; /* location of corner (left lower corner when angle = 0) */
  float y;
  float angle;

  /* Stuff calculated by forward kinematics */
  float c2_x;
  float c2_y;
  float c3_x;
  float c3_y;
  float c4_x;
  float c4_y;
  float c_angle;
  float s_angle;

} Block, *pBlock;

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

typedef struct simulator
{
  float time;  		        /* seconds */
  float time_step;   		/* seconds */

  pArm arms[MAX_N_ARMS];

  pBlock blocks[MAX_N_BLOCKS];

} Simulator, *pSimulator;

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

typedef struct communication_buffer
{

  int playing;
  float time;

  pSimulator ps;

} Communication_Buffer;

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