/*
 * pd.cc
 *
 * Daniel Lowd
 * Short Assignment #3
 * CS 154: Robotics
 * 2/10/03
 *
 * Implementation of PD-control for the Nomad 200.
 */

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

/* NOTE: I removed all references to Kbd, because it wouldn't work
 * under Linux.
 */

/* For some reason, I had to add the translational velocity to
 * the rotational velocities to make the robot go straight.
 * This only seems to be true for the Linux-compatible Nserver I
 * downloaded, and is contrary to all documentation I've read.
 * I created this macro to take care of that oddity.
 */
#define VM(a,b,c) vm(a, b+a, c+a)

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

/* Default proportional and derivative gains */
double P_GAIN = 0.5;
double D_GAIN = 0.5;

double MAX_SPEED = 300.0;
double DELAY = 1.0;


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

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

  ofstream outf("output");

  /* This line is required for use with the Linux Nserver I found. */
  strcpy(SERVER_MACHINE_NAME, "localhost");
  SERV_TCP_PORT = 7654; /* Port I choose to use. */

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

  int x_sp = 1000;         /* the set point (x = 1000 by default)        */
  int y_sp = 0;            /* the set point (y = 0 by default)           */

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

  int yplotLevel = 100;    /* the height of the recorded robot position  */
  int yplotDiff  = 30;     /* the additional height with eacah time step */

  /*
   * 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;

  /* different gains may be specified on the command line */
  if (argc > 1)            
  {
    P_GAIN = atof(argv[1]);
    cout << "Setting P_GAIN to " << P_GAIN << endl;
  }

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


  /* 
   * connect to the simulator
   */

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

  /* 
   * zero the robot
   */

  zr();

  /* 
   * turn on appropriate sensors 
   */

  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 */

  /*
   * draw the location of the goal point (set point)
   */
  draw_arc(x_sp-90,y_sp+90,180,180,0,3600,3);

  /*
   * 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 control loop
   */

  double te = 0.0;          /* the translational error */
  double td = 0.0;          /* derivative of distance from target */
  double last_te = x_sp;    /* former translational error, so we can
                             * compute the derivative of the error.
                             */
  double curr_x = 0.0;


  /* The only way for the user to exit this loop is via Ctrl-C or kill.
   * If we reach the target point or overshoot it, we'll exit automatically.
   */
  while (1) 
  {
    /*
     *  The following should guarantee that we update the
     *  robot velocity at most once per DELAY.
     */

    currentTime = T.getCurrentTime();

    if (currentTime > lastTime + DELAY)      /* DELAY has passed    */
    {
      curr_x = State[34];   /* 34 == x pos */

      /* proportional term */
      te = x_sp - curr_x;   

      /* derivative term */
      td = (te - last_te)/(currentTime - lastTime);

      /* PD control -- sum terms with gains to find new velocity. */
      tv = (int)(P_GAIN * te + D_GAIN * td);

      last_te = te;
      lastTime = T.getCurrentTime();         /* reset the timer     */
    }

    /* log to file */
    outf << currentTime-firstTime << " " << State[34] << endl;

    draw_arc(State[34]-5,yplotLevel+5,10,10,0,3600,3);
    yplotLevel += yplotDiff;

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

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

    gs();                                    /* update state        */

    /* Stop when we've made it. */ 
    if (tv == 0 && abs(State[34] - x_sp) < 2)
    {
        printf("Rise time: %fs\n", currentTime-firstTime);
        break;
    }

    /* If we've overshot, there's reason to continue.  We'll allow
     * an error of 0.1 in, just to be nice.
     */
    if (State[34] > x_sp + 1)
    {
        printf("OVERSHOOT\n");
        break;
    }
  }


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