/***********************************************************************
pendulum.c
Mimic ODE simulation for the one link pendulum in Assignment 1.

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

#include <stdio.h>
#include <math.h>

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

#define LENGTH (1.0f)   // length of pendulum
#define WIDTH (0.1f)    // width of pendulum
#define MASS (1.0f)     // mass of link

#define G (9.81f)

#define TIMESTEP 0.01

/* Score parameters */
#define VISCOUS_FRICTION 0.1
#define STATE_PENALTY 0.1

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

/* servo gains */
double k = 5.0;
double b = 1.0;

// moment of inertia of pendulum around joint.
double I_joint = MASS*(LENGTH*LENGTH + WIDTH*WIDTH)/12 + MASS*LENGTH*LENGTH/4;

double time = 0.0; // keep track of how much time has passed.

double score = 0; // keep track of the score so far.

FILE *data_file = NULL;

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

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

main()
{
  double angle_desired = M_PI;
  double angle = 0;
  double angular_velocity = 0;
  double new_angular_velocity = 0;
  double angular_acceleration = 0;
  double torque = 0;
  double total_torque = 0;
  double score = 0;
  int count = 0;

  data_file = fopen( "data", "w" );
  if ( data_file == NULL )
    {
      fprintf( stderr, "Can't open data file.\n" );
      exit( -1 );
    }

  /*
  printf( "I %20.15f, 0.5*mgl %20.15f\n", I_joint,
	  MASS*G*LENGTH*0.5 );
  */

  for( ; ; )
    {
      torque = -k*( angle - angle_desired ) - b*angular_velocity;
      total_torque = torque - VISCOUS_FRICTION*angular_velocity
	- MASS*G*LENGTH*0.5*sin(angle); 
      angular_acceleration = total_torque/I_joint;
      new_angular_velocity = angular_velocity + angular_acceleration*TIMESTEP;
      angle += (new_angular_velocity + angular_velocity)*TIMESTEP/2;
      angular_velocity = new_angular_velocity;
      time += TIMESTEP;
      score += torque*torque*TIMESTEP 
        + STATE_PENALTY*TIMESTEP
        *(angle - angle_desired)*(angle - angle_desired);

      if ( time < 3.0 )
	{
	  fprintf( data_file, "%g %g %g %g %g\n", time, angle, angular_velocity,
		   torque, score );
	}
      else
	{
	  fclose( data_file );
	  exit( -1 );
	}

      // Print data out as well
      count++;
      if ( (count % 100) == 0 )
	{
	  printf( "%g %g %g %g %g\n",
		  time, angle, angular_velocity, torque, score );
	}
    }
}