/*
 * pd.cc
 *
 * Feel free to use the makefile in this directory also.
 *
 */

#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include <stream.h>
#include <fstream.h>
#include <iostream.h>
#include "Timing.hh"
#include "KbdInput.hh"

extern "C" {
#include "Nclient.h"
}

double GAIN = 1.0;
double MAX_SPEED = 300.0;
double DELAY = 1.5;
double Kp = 1.0;
double Kd = 1.0;


/****************************
 *
 *  The usual main method
 *
 ****************************/

int
main(int argc, char** argv)
{

  ofstream outf("output"); /* a file for collecting data                 */

  SERV_TCP_PORT = 7098;    /* be sure to have the same port as your      */
                           /* server is expecting !                      */


  /*
   * tv is the constant translational velocity of the robot
   */

  int sp = 1000;           /* the set point (x = 1000 by default)        */

  int tv = 0;              /* zero 1/10s of an inch per second default   */


  /*
   * read in the file of parameters
   */

  char comment[100];
  ifstream inf("./parameters");
  inf >> MAX_SPEED >> comment;
  cout << "MAX_SPEED is " << MAX_SPEED << endl;
  inf >> DELAY >> comment;
  cout << "DELAY is " << DELAY << endl;


  /*
   * You may want to add additional command-line or parameter file options
   */

  if (argc > 1)
  {
    Kp = atof(argv[1]);
    cout << "Setting Kp to " << Kp << endl;
  }

  if (argc > 2)
  {
    Kd = atof(argv[2]);
    cout << "Setting Kd to " << Kd << endl;
  }

  if (argc > 3)
  {
    DELAY = atof(argv[3]);
    cout << "Setting DELAY to " << DELAY << endl;
  }

  if (argc > 4)   
  {
    GAIN = atof(argv[4]);
    cout << "Setting GAIN to " << GAIN << endl;
  }


  /* 
   * connect to the simulator
   */

  if (connect_robot(1)) 
  {
    printf("Connected to robot.\n");
    printf("Hit q to exit.\n");
  }
  else 
  {
    printf("Failed to connect to robot\n");
    exit(0);
  }


  /* 
   * zero the robot
   */

  zr();


  /* 
   * turn on appropriate sensors  -- not really needed, but OK
   */

  for (int i=1 ; i<=42 ; ++i)  Smask[i] = 1;
  ct();

  gs();                     /* update the robot's state   */

  int rv = 0;               /* start going straight ahead */

  vm(tv,rv,rv);             /* set the initial velocities */

  /*
   * the Timing object is just to avoid remembering the system
   *   call that returns the time...
   */

  Timing T;
  double firstTime = T.getCurrentTime();
  double lastTime = 0.0;
  double currentTime;


  /*
   * the KbdInput object allows nonblocking input
   */

  KbdInput KI; 
  char c; 


  /*
   * the control loop
   */

  KI.on();                  /* turn on raw input/output             */

  double prop   = 0.0;
  double ei_1   = 0.0;
  double ei_2   = 0.0;
  double ei     = 0.0;
  double derive = 0.0;

  while (1) 
  {
    c = KI.nextChar();      /* c is 0 if there's no input ready     */
  
    if ( c == 'q' )         /* hit q in the terminal window to quit */
    {
       break;
    }

    /*
     *  The following should guarantee that we update the
     *  robot velocity at most once per DELAY time.
     */

    currentTime = T.getCurrentTime();

    if (currentTime > lastTime + DELAY)      /* a second has passed */
    {
      lastTime = T.getCurrentTime();         /* reset the timer     */

      /*
       * Here you will want to compute the error and the velocity
       *   according to the control strategy you're using --
       *   either proportional or PD control.
       */

      /*
       * Prob 1
       */

      ei_2 = ei_1;
      ei_1 = ei;
      ei   = 1000 - State[34];
      prop = Kp * ei;

      derive = ei - (2 * ei_1) + ei_2; 
      
      double result = prop + (Kd * derive);

      tv = (int)result;

      /* setting randomly    */

      if (tv >  MAX_SPEED) {tv =  MAX_SPEED;}/* keep within limits  */
      if (tv < -MAX_SPEED) {tv = -MAX_SPEED;}
    }

    outf << currentTime-firstTime << " " << State[34] << endl;

    vm(tv,rv,rv);                            /* set velocity        */

    gs();                                    /* update state        */

  }


  KI.off();                 /* reset terminal settings              */

  disconnect_robot(1);      /* gracefully disconnect from the robot */

}