/************************************************************************/
/*
graphics.c: Draw the biped walking
*/
/************************************************************************/

#include <stdio.h>
#include <math.h>
#include "../graphics-x11/pixels.h"
#include "../graphics-x11/views.h"

#include "compass1.h"

extern float last_x1, last_y1, last_x2, last_y2;

static float left_x1, left_y1, left_x2, left_y2;
static float right_x1, right_y1, right_x2, right_y2;

static int first_time = 1;

static View v, *pv;

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

void display_stuff()
{
  extern double xt[];
  extern double left_foot_position[];
  extern double right_foot_position[];

  vdraw_line( -1.0, 0.0, 11.0, 0.0, SET, pv );
  vdraw_line( 0.0, -0.1, 0.0, 1.1, SET, pv );
  vdraw_line( 10.0, -0.1, 10.0, 1.1, SET, pv );

  vdraw_line( xt[HIP_X], xt[HIP_Y], 
	      left_foot_position[0], left_foot_position[1],
	      SET, pv );
  left_x1 = last_x1;
  left_y1 = last_y1;
  left_x2 = last_x2;
  left_y2 = last_y2;
  vdraw_line( xt[HIP_X], xt[HIP_Y], 
	      right_foot_position[0], right_foot_position[1],
	      SET, pv );
  right_x1 = last_x1;
  right_y1 = last_y1;
  right_x2 = last_x2;
  right_y2 = last_y2;

  /*
  printf( "%g %g %g %g %g %g\n", xt[HIP_X], xt[HIP_Y], 
	  left_foot_position[0], left_foot_position[1],
	  right_foot_position[0], right_foot_position[1] );
  */
  flush_graphics();
}

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

void erase_stuff()
{
  _vdraw_line( right_x1, right_y1, right_x2, right_y2, CLEAR );
  _vdraw_line( left_x1, left_y1, left_x2, left_y2, CLEAR );
}

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

void redisplay_stuff()
{
  static int count = 0;

  count++;
  if ( count < 30 )
    return;
  count = 0;

  erase_stuff();
  display_stuff();
}

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

void reinit_display()
{
  clear_window();
  display_stuff();
  first_time = 1;
}

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

void init_my_graphics()
{

  init_window( 10.0 );
  pv = &v;
  set_up_view( pv, -1.0, -0.1, 11.0, 1.1 );
  reinit_display();
}

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