/*****************************************************************************
 *                                                                           *
 * FOX Controller Library, Version 1.3.3.                                    *
 *                                                                           *
 * Copyright (C) 1998,1999,2000 Russell Smith (rl.smith@auckland.ac.nz)      *
 *                                                                           *
 * The FOX Controller Library is free software; you can redistribute it      *
 * and/or modify it under the terms of the GNU Library General Public        *
 * License as published by the Free Software Foundation; either version      *
 * 2 of the License, or (at your option) any later version.                  *
 *                                                                           *
 * The FOX Controller Library is distributed in the hope that it will be     *
 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of    *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU         *
 * Library General Public License for more details.                          *
 *                                                                           *
 * You should have received a copy of the GNU Library General Public         *
 * License along with the Fox Controller Library; see the file COPYING.LIB.  *
 * If not, write to the Free Software Foundation, Inc., 59 Temple Place -    *
 * Suite 330, Boston, MA 02111-1307, USA.                                    *
 *                                                                           *
 *****************************************************************************/

/*

Test of the FOX-N algorithm on linear gantry crane system.

The crane model is two masses (m1 and m2) on a line with a spring connecting
them. m1 is the cart and m2 is the pendulum. The state variables are
x1,x2,dx1,dx2. The equations of motion are:

  ddx1 = (x2-x1+f)/m1		(ddx1 = d^2(x1)/dt^2)
  ddx2 = (x1-x2)/m2

The system includes an "internal" PD controller which adds an internal force
fint to f :

  f = fext + fint
  fint = a*x1 + b*x2 + c*dx1 + d*dx2

The cart is subject to coloumb friction.

*/

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

#include "fox-n.h"


cftype frandom()
{
  return cftype(random())/cftype(RAND_MAX);
}


cftype softsign (cftype x)
{
  x *= 100.0;
  if (x > 1.0) return 1.0;
  else if (x < -1.0) return -1.0;
  else return x;
}


void PlotData (int n, float *y)
{
  for (int i=0; i<n; i++) {
    printf ("%.8e ",y[i]);
  }
  printf ("\n");
}


// simulation parameters
const cftype kh=0.1;
const int T=400;		// total number of time steps
const cftype m1 = 5;
const cftype m2 = 1;
// CmacInputInfo cmac_input_info[1] = {{T,0,T*kh}};
CmacInputInfo cmac_input_info[4] = {
  {200,-5,5},
  {200,-2,2},
  {200,-5,5},
  {200,-2,2}};

// internal controller stuff

const cftype cka = 0;
const cftype ckb = 0;
const cftype ckc = -7;
const cftype ckd = 5;

// fox system model parameters

const cftype ka=0.5;
const cftype kb=0.9;		// was 2
cftype Adata[4] = {1,kh,-ka*kh,1-kb*kh};
cftype Bdata[2] = {0,kh*ka};
cftype Cdata[2] = {1,0};

//const cftype kprop = 2;
//const cftype kderiv = 10;
//const cftype ka = -1.0 - kprop;
//const cftype kb = 0 + kprop;
//const cftype kc = -5.0 - kderiv;
//const cftype kd = 0 + kderiv;

// cftype Adata[16] = {
// 	1, 0, kh, 0,
// 	0, 1, 0, kh,
// 	((ka-1)*kh/m1), ((kb+1)*kh/m1), (1.0+(kc*kh/m1)), (kd*kh/m1),
// 	(kh/m2), (-kh/m2), 0, 1};
// cftype Bdata[4] = {0, 0, kh/m1, 0};
// cftype Cdata[4] = {0, 1, 0, 0};

// cftype Adata[9] = {
//     1.0000,   -0.0000,    0.1000,
//    -0.7000,    1.5000,   -0.5000,
//     -0.7200,    0.5200,    0.3600};
// cftype Bdata[3] = {0, 0, 0.0200};
// cftype Cdata[3] = {-1,0,0};

// misc parameters
const float yscale = 5.0;


int main (int argc, char *argv[])
{
  // initialize random numbers
  //srandom(1234567);
  srandom(time(NULL));

//   // make random-ish xref trajectory to follow
//   for (int i=0; i<T; i++) xref[i] = 0;
//   for (int i=0; i<10; i++) {
//     cftype xc = frandom()*cftype(T)*kh;
//     cftype gain = 10*(frandom()-0.5);
//     for (int j=0; j<T; j++)
//       xref[j] += 1.0/(1.0+exp(-((cftype(j)*kh)-xc)*gain));
//   }
//   // starts at 0
//   for (int i=1; i<T; i++) xref[i] -= xref[0];
//   xref[0] = 0;
  cftype xref[T];
  for (int i=0; i<T; i++) xref[i] = 6.0 * ((i % 200) > 100) - 3.0;

  PlotData (T,xref);

  // setup fox
  const int num_inputs = 4;
  const int num_weights = 100000;
  const int history = 100; //150;
  const int C = 10;
  const int ny = 2; //3;
  FoxN fox (num_inputs,cmac_input_info,num_weights,C,ny,
	    Adata,Bdata,Cdata,history);
  fox.SetEligibilityMode (1);

  // setup storage
  cftype *this_x1 = new cftype [T];
  cftype *this_x2 = new cftype [T];
  cftype *this_dx2 = new cftype [T];
  cftype *this_des_dx2 = new cftype [T];
  cftype *this_f = new cftype [T];
  cftype *save_x1 = new cftype [T];
  cftype *save_x2 = new cftype [T];
  cftype *save_dx2 = new cftype [T];
  cftype *save_des_dx2 = new cftype [T];
  cftype *save_f = new cftype [T];
  for (int i=0; i<T; i++)
    this_x1[i] = this_x2[i] = this_dx2[i] = this_des_dx2[i] = 0;
  for (int i=0; i<T; i++)
    save_x1[i] = save_x2[i] = save_dx2[i] = save_des_dx2[i] = 0;
  for (int i=0; i<T; i++) save_f[i] = 0;

  for (int iteration=0; iteration<2000000; iteration++) {
    cftype x1,x2,dx1,dx2;
    x1 = 0;
    x2 = 0;
    dx1 = 0;
    dx2 = 0;
    fox.Reset();

    cftype min[4],max[4];
    for (int i=0; i<4; i++) min[i] = 1e30;
    for (int i=0; i<4; i++) max[i] = -1e30;

    for (int i=0; i<T; i++) {
      // reset fox eligibilities when xref changes
      if ((i % 200) == 0 || (i % 200)==100) {
	fox.Reset();
      }

      // find state variable ranges
      min[0] = min[0] <? x1;
      min[1] = min[1] <? x2;
      min[2] = min[2] <? dx1;
      min[3] = min[3] <? dx2;
      max[0] = max[0] >? x1;
      max[1] = max[1] >? x2;
      max[2] = max[2] >? dx1;
      max[3] = max[3] >? dx2;

      // get desired dx2
      cftype desired_dx2 = 1.5 * atan (1.0*(xref[i] - x2));

      // cftype input = cftype(i)*kh + kh/2.0;
      cftype input[4];
      input[0] = x1 - xref[i];
      input[1] = dx1;
      input[2] = x2 - xref[i];
      input[3] = dx2;
      fox.Map (input);

      cftype fint = cka*x1 + ckb*x2 + ckc*dx1 + ckd*dx2;

      cftype f = fint + fox.Output();
      // if (i > 250) f = 0;		// to check cart friction

      this_x1[i] = x1;
      this_x2[i] = x2;
      this_dx2[i] = dx2;
      this_des_dx2[i] = desired_dx2;
      this_f[i] = f;

      cftype error = (desired_dx2 - dx2);
      // if (fabs(error) > 5.0) error = 0.0;	// Important!
      cftype learning_rate = 0.1;
      // fox.LimitOutput (learning_rate * 0.005);
      // fox.LimitOutputDerivative (learning_rate * 0.01);
      // fox.Train (error * learning_rate);

      // penalize overshoots
      static cftype old_error = 0;
      if (fabs(error) > fabs(old_error)) fox.Train (error * learning_rate);
      else fox.Train (error * learning_rate * 0.1);
      old_error = error;

      cftype ddx1,ddx2;
      cftype cff;			// cart coloumb friction force

      cff = -1.0 * softsign (dx1);
      ddx1 = (x2 - x1 + f + cff) / m1;	// divide by mass m1
      ddx2 = (x1 - x2) / m2;		// divide by mass m2
      dx1 += kh * ddx1;
      dx2 += kh * ddx2;
      x1 += kh * dx1;
      x2 += kh * dx2;
    }

    // plot the results
    if ((iteration % 1)==0) {
      fprintf (stderr,"%d\n",iteration);
      fprintf (stderr,"min = %f %f %f %f\n",min[0],min[1],min[2],min[3]);
      fprintf (stderr,"max = %f %f %f %f\n",max[0],max[1],max[2],max[3]);

#define DOPLOT(var) \
  PlotData (T,this_ ## var); \
  tmp = this_ ## var; \
  this_ ## var = save_ ## var; \
  save_ ## var = tmp;

      cftype *tmp;
      DOPLOT (x1);
      DOPLOT (x2);
      DOPLOT (dx2);
      DOPLOT (des_dx2);
      DOPLOT (f);
    }
  }

  return 0;
}