/*
Com S 476/576 Course Software 
Release 0.1  02/22/00

Copyright 1999,2000

Steven M. LaValle, Iowa State University
*/

#include <fstream.h>
#include <math.h>

#include <LEDA/file.h>
#include <LEDA/list.h>
#include <LEDA/matrix.h>
#include <LEDA/point.h>
#include <LEDA/polygon.h>
#include <LEDA/random.h>
#include <LEDA/stream.h>
#include <LEDA/string.h>
#include <LEDA/vector.h>

#include "model.hh"

#ifndef PI
#define PI 3.1415926535897932385
#endif

#ifndef INFINITY
#define INFINITY 1.0e40
#endif

#ifndef sqr
#define sqr(x) ((x)*(x))
#endif

#ifndef min
#define min(x,y) ((x<y) ? x : y)
#endif

#define NUM_INPUTS_2DPOINT 50
#define NUM_INPUTS_2DRIGID 8
#define NUM_INPUTS_3D 8
#define NUM_INPUTS_3D_FORWARD 16

leda_vector Model::RungeKuttaIntegrate(const leda_vector &x, const leda_vector &u, 
		  const double &h) 
{
  leda_vector k1,k2,k3,k4;

  k1 = StateTransitionEquation(x,u);
  k2 = StateTransitionEquation(x + (0.5*h)*k1,u);
  k3 = StateTransitionEquation(x + (0.5*h)*k2,u);
  k4 = StateTransitionEquation(x + h*k3,u);

  return x + h/6.0*(k1 + 2.0*k2 + 2.0*k3 + k4);
}



leda_vector Model::EulerIntegrate(const leda_vector &x, const leda_vector &u, 
		  const double &h) 
{
  return x + h*StateTransitionEquation(x,u);
}



leda_list<leda_vector> Model::GetInputs(const leda_vector &x) {
  return Inputs;
}


leda_point Model::StateToPoint(const leda_vector &x) {
  return leda_point(x[0],x[1]);
}



// By default, don't change anything
leda_vector Model::StateToConfiguration(const leda_vector &x) {
  return x;
}



// Overrides the default Inputs by reading from a file
void Model::ReadInputs() {
  Inputs.clear();
  file_istream fin(FilePath + "Inputs");
  fin >> Inputs;
}



void Model::ReadLowerState() {
  file_istream fin(FilePath + "LowerState");
  fin >> LowerState;
}



void Model::ReadUpperState() {
  file_istream fin(FilePath + "UpperState");
  fin >> UpperState;
}


bool Model::Satisfied(const leda_vector &x) {
  return true;
}


// Default is to use the standard Euclidean metric
double Model::Metric(const leda_vector &x1, const leda_vector &x2) {

  double rho;

  rho = (x1 - x2).length();

  return rho;
}


// Some models will interpolate differently because of 
// topology (e.g., S^1, P^3)
leda_vector Model::LinearInterpolate(const leda_vector &x1, const leda_vector &x2, 
				const double &a) {
  return a*x1 + (1.0-a)*x2;
}





// *********************************************************************
// *********************************************************************
// CLASS:     Model::ModelLinear
// 
// *********************************************************************
// *********************************************************************

ModelLinear::ModelLinear(leda_string path = ""):Model(path) {

  leda_vector v;
  int i;

  if ((path.length() > 0)&&(path[path.length()-1] != '/'))
    path += "/";

  FilePath = path;

  file_istream fin(FilePath + "A");
  fin >> A;
  file_istream fin2(FilePath + "B");
  fin2 >> B;

  StateDim = A.dim1();
  InputDim = B.dim2();

  if (is_file(FilePath+"LowerState"))
    ReadLowerState();
  else {
    LowerState = leda_vector(StateDim);
    for (i = 0; i < StateDim; i++) {
      LowerState[i] -= 10.0;
    }
  }

  if (is_file(FilePath+"UpperState"))
    ReadUpperState();
  else {
    UpperState = leda_vector(StateDim);
    for (i = 0; i < StateDim; i++) {
      UpperState[i] += 10.0;
    }
  }

  if (is_file(FilePath+"Inputs"))
    ReadInputs();
  else {
    Inputs.clear();  // Just to be safe
    v = leda_vector(InputDim);  // Initializes to the zero leda_vector
    Inputs.push(v);
    for (i = 0; i < StateDim; i++) {
      v = leda_vector(InputDim);
      v[i] = 1.0;
      Inputs.push(v);
      v[i] = -1.0;
      Inputs.push(v);
    }
  }

}


leda_point ModelLinear::StateToPoint(const leda_vector &x) {
  return leda_point(5.0*(x[0]+10.0),5.0*(x[1]+10.0));
}

leda_vector ModelLinear::StateToConfiguration(const leda_vector &x) {
  return leda_vector(5.0*(x[0]+10.0),5.0*(x[1]+10.0)); 
}


leda_vector ModelLinear::Integrate(const leda_vector &x, const leda_vector &u, const double &h)
{
  return RungeKuttaIntegrate(x,u,h);
}


leda_vector ModelLinear::StateTransitionEquation(const leda_vector &x, const leda_vector &u)
{
  leda_vector dx(StateDim);

  dx = A * x + B * u; // This looks too easy!

  return dx;
}



// *********************************************************************
// *********************************************************************
// CLASS:     Model::Model2D
// 
// *********************************************************************
// *********************************************************************



Model2D::Model2D(leda_string path = ""):Model(path) {

  Speed = 1.0;

  if ((path.length() > 0)&&(path[path.length()-1] != '/'))
    path += "/";

  FilePath = path;
  
}



// *********************************************************************
// *********************************************************************
// CLASS:     Model::Model2DPoint
// 
// *********************************************************************
// *********************************************************************


// Constructor
Model2DPoint::Model2DPoint(leda_string path = ""):Model2D(path) {
  double theta;

  StateDim = 2;
  InputDim = 2;

  if (is_file(FilePath+"LowerState"))
    ReadLowerState();
  else {
    LowerState = leda_vector(-70.0,-70.0);
  }

  if (is_file(FilePath+"UpperState"))
    ReadUpperState();
  else {
    UpperState = leda_vector(70.0,70.0);
  }

  // Make the list of Inputs
  if (is_file(FilePath+"Inputs"))
    ReadInputs();
  else {
    for (theta = 0.0; theta < 2.0*PI; theta += 2.0*PI/NUM_INPUTS_2DPOINT) {
      Inputs.push_back(leda_vector(cos(theta),sin(theta)));
    }
  }
}




leda_vector Model2DPoint::StateTransitionEquation(const leda_vector &x, const leda_vector &u) {

  leda_vector dx(2);

  dx = Speed*u;
  return dx;
}



double Model2DPoint::Metric(const leda_vector &x1, const leda_vector &x2) {

  double rho;

  rho = (x1 - x2).length();

  return rho;
}





leda_vector Model2DPoint::Integrate(const leda_vector &x, const leda_vector &u, const double &h)
{
  return EulerIntegrate(x,u,h);
}




// *********************************************************************
// *********************************************************************
// CLASS:     Model::Model2DPointCar
// 
// *********************************************************************
// *********************************************************************


// Constructor
Model2DPointCar::Model2DPointCar(leda_string path = ""):Model2DPoint(path) {
  double alpha;

  StateDim = 3;
  InputDim = 2;

  if (is_file(FilePath+"LowerState"))
    ReadLowerState();
  else {
    LowerState = leda_vector(0.0,0.0,0.0);
  }

  if (is_file(FilePath+"UpperState"))
    ReadUpperState();
  else {
    UpperState = leda_vector(100.0,100.0,2.0*PI);
  }

  MaxSteeringAngle = PI/6.0;
  CarLength = 4.0;

  // Make the list of Inputs
  Inputs.clear();  // Otherwise its parent constructor will make some inputs
  if (is_file(FilePath+"Inputs"))
    ReadInputs();
  else {
    for (alpha = -MaxSteeringAngle; alpha <= MaxSteeringAngle; 
	 alpha += 2.0*MaxSteeringAngle/6.0) {
      Inputs.push_back(leda_vector(1.0,alpha));
      Inputs.push_back(leda_vector(-1.0,alpha)); 
    }
  }
}



leda_vector Model2DPointCar::StateTransitionEquation(const leda_vector &x, const leda_vector &u) {

  leda_vector dx(3);

  dx[0] = Speed*u[0]*cos(x[2]);
  dx[1] = Speed*u[0]*sin(x[2]);
  dx[2] = tan(u[1])/CarLength;
  return dx;
}



double Model2DPointCar::Metric(const leda_vector &x1, const leda_vector &x2) {

  double rho,dtheta;
  
  dtheta = min(fabs(x1[2]-x2[2]),2.0*PI - fabs(x1[2]-x2[2]));

  rho = sqrt(sqr(x1[0] - x2[0]) + sqr(x1[1] - x2[1]) + 50.0/PI*sqr(dtheta));

  return rho;
}



leda_vector Model2DPointCar::Integrate(const leda_vector &x, const leda_vector &u, double const &h) {
  leda_vector nx(3);

  nx = RungeKuttaIntegrate(x,u,h);

  // Make sure the S^1 topology is preserved for 2D rotation
  if (nx[2] > 2.0*PI)
    nx[2] -= 2.0*PI;
  if (nx[2] < 0.0)
    nx[2] += 2.0*PI;

  return nx;
}


// *********************************************************************
// *********************************************************************
// CLASS:     Model::Model2DRigid
// 
// *********************************************************************
// *********************************************************************



// Constructor
Model2DRigid::Model2DRigid(leda_string path = ""):Model2D(path) {
  double theta;
  Speed = 1.0;
  StateDim = 3;
  InputDim = 3;

  if (is_file(FilePath+"LowerState"))
    ReadLowerState();
  else {
    LowerState = leda_vector(-70.0,-70.0,0.0);
  }

  if (is_file(FilePath+"UpperState"))
    ReadUpperState();
  else {
    UpperState = leda_vector(70.0,70.0,2.0*PI);
  }

  // Make the list of Inputs
  if (is_file(FilePath+"Inputs"))
    ReadInputs();
  else {
    for (theta = 0.0; theta < 2.0*PI; theta += 2.0*PI/NUM_INPUTS_2DRIGID) {
    //for (theta = -0.5*PI; theta < 0.5*PI; theta += PI/NUM_INPUTS_2DRIGID) {
     Inputs.push_back(leda_vector(cos(theta),sin(theta),0.0));
      //Inputs.push_back(leda_vector(cos(theta),sin(theta),-0.2));
      //Inputs.push_back(leda_vector(cos(theta),sin(theta),0.2));
    }
    Inputs.push_back(leda_vector(0.0,0.0,-0.3));
    Inputs.push_back(leda_vector(0.0,0.0,0.3));
  }

}


leda_vector Model2DRigid::StateTransitionEquation(const leda_vector &x, const leda_vector &u) {

  leda_vector dx(3);

  dx = Speed*u;
  return dx;
}



double Model2DRigid::Metric(const leda_vector &x1, const leda_vector &x2) {

  double fd = fabs(x1[2]-x2[2]);
  double dtheta = min(fd,2.0*PI - fd);

  return sqrt(sqr(x1[0] - x2[0]) + sqr(x1[1] - x2[1]) + sqr(50.0/PI*dtheta));
  double rho;
  //return sqrt(sqr(x1[0] - x2[0]) + sqr(x1[1] - x2[1]));
  //rho = (x1 - x2).length();
  
  //return rho;
}



// Handle S^1 topology properly (for rotation)
leda_vector Model2DRigid::LinearInterpolate(const leda_vector &x1, const leda_vector &x2, 
				const double &a) {

  leda_vector v;

  v = a*x1 + (1.0-a)*x2;

  if (x2[2] - x1[2] > PI) {
    if (x1[2] > x2[2])
      v[2] = a*x1[2] + (1.0-a)*(x2[2]+PI);
    else
      v[2] = a*(x1[2]+PI) + (1.0-a)*x2[2];
  }

  if (v[2] > 2.0*PI)
    v[2] -= 2.0*PI;

  cout<<"Model2DRigid::LinearInterpolate"<<endl;
  return v;
}



leda_vector Model2DRigid::Integrate(const leda_vector &x, const leda_vector &u, const double &h)
{
  leda_vector nx(3);

  nx = RungeKuttaIntegrate(x,u,h);

  // Make sure the S^1 topology is preserved for 2D rotation
  if (nx[2] > 2.0*PI)
    nx[2] -= 2.0*PI;
  if (nx[2] < 0.0)
    nx[2] += 2.0*PI;

  return nx;
}




// *********************************************************************
// *********************************************************************
// CLASS:     Model::Model2DRigidCar
// 
// *********************************************************************
// *********************************************************************


// Constructor
Model2DRigidCar::Model2DRigidCar(leda_string path = ""):Model2DRigid(path) {
  double alpha;
  Speed = 2.5;
  StateDim = 3;
  InputDim = 2;

  if (is_file(FilePath+"LowerState"))
    ReadLowerState();
  else {
    LowerState = leda_vector(-70.0,-70.0,0.0);
  }

  if (is_file(FilePath+"UpperState"))
    ReadUpperState();
  else {
    UpperState = leda_vector(70.0,70.0,2.0*PI);
  }

  MaxSteeringAngle = PI/12.0;
  CarLength = 1.0;

  // Make the list of Inputs
  Inputs.clear();  // Otherwise its parent constructor will make some inputs
  if (is_file(FilePath+"Inputs"))
    ReadInputs();
  else {
    for (alpha = -MaxSteeringAngle; alpha <= MaxSteeringAngle; 
	 alpha += 2.0*MaxSteeringAngle/6.0) {
      Inputs.push_back(leda_vector(1.0,alpha)); 
      Inputs.push_back(leda_vector(-1.0,alpha)); 
    }
  }
}


leda_vector Model2DRigidCar::StateTransitionEquation(const leda_vector &x, const leda_vector &u) {

  leda_vector dx(3);
  dx[0] = Speed*u[0]*cos(x[2]);
  dx[1] = Speed*u[0]*sin(x[2]);
  dx[2] = Speed*u[0]*tan(u[1])/CarLength;
  return dx;
}


// *********************************************************************
// *********************************************************************
// CLASS:     Model::Model2DRigidCarForward
// 
// *********************************************************************
// *********************************************************************

Model2DRigidCarForward::Model2DRigidCarForward(leda_string path = ""):Model2DRigidCar(path) {
  double alpha;

  StateDim = 3;
  InputDim = 2;

 // Defaults are inherited
  if (is_file(FilePath+"LowerState"))
    ReadLowerState();
  if (is_file(FilePath+"UpperState"))
    ReadUpperState();

  // Make the list of Inputs
  Inputs.clear();  // Otherwise its parent constructor will make some inputs
  if (is_file(FilePath+"Inputs"))
    ReadInputs();
  else {
    for (alpha = -MaxSteeringAngle; alpha <= MaxSteeringAngle; 
	 alpha += 2.0*MaxSteeringAngle/6.0) {
      Inputs.push_back(leda_vector(1.0,alpha)); 
    }
  }
}


// *********************************************************************
// *********************************************************************
// CLASS:     Model::Model2DRigidSmooth
//  Smooth steering 
// *********************************************************************
// *********************************************************************


Model2DRigidCarSmooth::Model2DRigidCarSmooth(leda_string path = ""):Model2DRigidCar(path) {

  StateDim = 4;
  InputDim = 2;
  SteeringSpeed = 0.05;  // Default 0.05
  
  LowerState = leda_vector(4);
  UpperState = leda_vector(4);

  if (is_file(FilePath+"LowerState"))
    ReadLowerState();
  else {
    LowerState[0] = 0.0; LowerState[1] = 0.0; LowerState[2] = 0.0; 
    LowerState[3] = -MaxSteeringAngle;
  }

  if (is_file(FilePath+"UpperState"))
    ReadUpperState();
  else {
    UpperState[0] = 100.0; UpperState[1] = 100.0; UpperState[2] = 2.0*PI; 
    UpperState[3] = MaxSteeringAngle;
  }

  // Make the list of Inputs
  Inputs.clear();  // Otherwise its parent constructor will make some inputs
  if (is_file(FilePath+"Inputs"))
    ReadInputs();
  else {
    Inputs.push_back(leda_vector(1.0,0.0)); // Keep the steering angle fixed
    Inputs.push_back(leda_vector(-1.0,0.0)); // Keep the steering angle fixed
    Inputs.push_back(leda_vector(1.0,SteeringSpeed)); 
    Inputs.push_back(leda_vector(-1.0,SteeringSpeed)); 
    Inputs.push_back(leda_vector(1.0,-SteeringSpeed)); 
    Inputs.push_back(leda_vector(-1.0,-SteeringSpeed)); 
   }
}



leda_vector Model2DRigidCarSmooth::StateTransitionEquation(const leda_vector &x, const leda_vector &u) {

  leda_vector dx(4);

  dx[0] = Speed*u[0]*cos(x[2]);
  dx[1] = Speed*u[0]*sin(x[2]);
  dx[2] = Speed*u[0]*tan(x[3])/CarLength;
  dx[3] = u[1];

  //cout << "DX: " << dx << "\n";

  return dx;
}






leda_vector Model2DRigidCarSmooth::Integrate(const leda_vector &x, const leda_vector &u, const double &h)
{
  return RungeKuttaIntegrate(x,u,h);
}


double Model2DRigidCarSmooth::Metric(const leda_vector &x1, const leda_vector &x2) {

  double rho,dphi,dtheta;

  dphi = min(fabs(x1[3]-x2[3]),2.0*PI - fabs(x1[3]-x2[3]));
  dtheta = min(fabs(x1[2]-x2[2]),2.0*PI - fabs(x1[2]-x2[2]));

  rho = sqrt(sqr(x1[0] - x2[0]) + sqr(x1[1] - x2[1]) +
    sqr(2.0/PI*dphi) +
    sqr(50.0/PI*dtheta));

  return rho;
}



// *********************************************************************
// *********************************************************************
// CLASS:     Model::Model2DRigidDyncar
//  
// *********************************************************************
// *********************************************************************

// Constructor
Model2DRigidDyncar::Model2DRigidDyncar(leda_string path = ""):Model2DRigid(path) {
  double alpha;
  leda_vector v;

  StateDim = 5;
  InputDim = 1;
  Mass = 100.0;
  CAF = 17000.0;
  CAR = 20000.0;
  Adist = 4.0;
  Bdist = 5.0;
  Izz = 1600.0;

  WorldScale = 0.1;
  Speed = 88.0;   // Feet per sec
  
  LowerState = leda_vector(5);
  UpperState = leda_vector(5);

  if (is_file(FilePath+"LowerState"))
    ReadLowerState();
  else {
    LowerState[0] = -50.0; LowerState[1] = -5.0; LowerState[2] = 0.0; 
    LowerState[3] = -1000.0; LowerState[4] = 0.0;
  }

  if (is_file(FilePath+"UpperState"))
    ReadUpperState();
  else {
    UpperState[0] = 50.0; UpperState[1] = 5.0; UpperState[2] = 1000.0; 
    UpperState[3] = 0.0; UpperState[4] = 2.0*PI;
  }

  MaxSteeringAngle = 0.6;

  // Make the list of 1D Inputs
  Inputs.clear();  // Otherwise its parent constructor will make some inputs
  if (is_file(FilePath+"Inputs"))
    ReadInputs();
  else {
    for (alpha = -MaxSteeringAngle; alpha <= MaxSteeringAngle; 
	 alpha += 2.0*MaxSteeringAngle/24.0) {
      v = leda_vector(1); v[0] = alpha;
      Inputs.push_back(v);
    }
  }
}


// This is xdot = f(x,u) for a 5dof state-space, and 1dof input
//  Model taken from Jim Bernard...
// alphaf = (v + a*r)/u - deltaf
// alphar = (v-b*r)/u
// FYF = -CAF*alphaf
// FYR = -CAR*alphar
// vdot = -u*r  + (FYF + FYR)/m
// rdot = (FYF*a - FYR*b)/Izz
// Xdot = u*cos(psi) - v*sin(psi)
// Ydot = u*sin(spi) + v*cos(psi)
// psidot = r
// 
// m = mass of car, say about 100 slugs
// CAF = front cornering stiffness in pounds per radiaof the tires, say about
// 17000
// CAR = rear cornering stiffness, say about 20000
// a is dist from mass center to front tires, say 4 feet
// b is dist from mass center to rear tires, say 5 feet
// Izz is yaw moment of intertia, say about 1600 slug ft**2
// u is forward speed which is assumed constant, input is in feet/sec
// delta is your input, it is the steer angle of the tires in radians.

leda_vector Model2DRigidDyncar::StateTransitionEquation(const leda_vector &x, const leda_vector &u)
{
  double alphaf,alphar,fyf,fyr,v,r,psi;
  leda_vector dx(5);

  v = x[0]; r = x[1]; psi = x[4];

  alphaf = (v + Adist * r) / Speed - u[0];
  alphar = (v - Bdist * r) / Speed;
  fyf = -CAF * alphaf;
  fyr = -CAR * alphar;

  /* Transfer the velocity */
  dx[0] = -Speed * r  + (fyf + fyr) / Mass;
  dx[1] = (fyf * Adist - fyr * Bdist) / Izz;
  dx[2] = Speed * cos(psi) - v * sin(psi);
  dx[3] = Speed * sin(psi) + v * cos(psi);
  dx[4] = r;

  //cout << "x: " << x << "  Dx: " << dx << "  u: " << u[0] << "\n";

  return dx;
}



double Model2DRigidDyncar::Metric(const leda_vector &x1, const leda_vector &x2) {
  double d = 0.0;
  int i;

  for (i = 0; i < 5; i++) {
    d += sqrt(sqr(x1[i] - x2[i]) / (UpperState[i] - LowerState[i]));
  }

  return d;
}



leda_vector Model2DRigidDyncar::Integrate(const leda_vector &x, const leda_vector &u, const double &h)
{
  return RungeKuttaIntegrate(x,u,h);
}



leda_point Model2DRigidDyncar::StateToPoint(const leda_vector &x) {
  return leda_point(x[2]*WorldScale,-x[3]*WorldScale);
}



// NOTE: This neglects the S^1 effect of orientation!!
leda_vector Model2DRigidDyncar::StateToConfiguration(const leda_vector &x) {
  return leda_vector(x[2]*WorldScale,-x[3]*WorldScale,-x[4]); 
}



// *********************************************************************
// *********************************************************************
// CLASS:     Model::Model2DRigidLander
//  
// *********************************************************************
// *********************************************************************

// Constructor
Model2DRigidLander::Model2DRigidLander(leda_string path = ""):Model2DRigid(path) {
  leda_vector v;

  StateDim = 4;
  InputDim = 1;
  Mass = 1.0;
  G = 1.568;  // Accel of gravity on moon (use 9.8 for earth)
  Fs = 10.0;
  Fu = 20.0;

  LowerState = leda_vector(4);
  UpperState = leda_vector(4);

  if (is_file(FilePath+"LowerState"))
    ReadLowerState();
  else {
    LowerState[0] = 0.0; LowerState[1] = 0.0; LowerState[2] = -10.0; 
    LowerState[3] = -10.0; 
  }

  if (is_file(FilePath+"UpperState"))
    ReadUpperState();
  else {
    UpperState[0] = 100.0; UpperState[1] = 100.0; UpperState[2] = 10.0; 
    UpperState[3] = 10.0; 
  }

  // Make the list of 1D Inputs
  Inputs.clear();  // Otherwise its parent constructor will make some inputs
  if (is_file(FilePath+"Inputs"))
    ReadInputs();
  else {
    v = leda_vector(1); v[0] = 0;
    Inputs.push_back(v);
    v = leda_vector(1); v[0] = 1;
    Inputs.push_back(v);
    v = leda_vector(1); v[0] = 2;
    Inputs.push_back(v);
    v = leda_vector(1); v[0] = 3;
    Inputs.push_back(v);
  }
}



leda_vector Model2DRigidLander::Integrate(const leda_vector &x, const leda_vector &u, const double &h)
{
  return RungeKuttaIntegrate(x,u,h);
}



leda_vector Model2DRigidLander::StateToConfiguration(const leda_vector &x) {
  return leda_vector(x[0],x[1],0.0); // Yield a zero rotation every time
}


leda_vector Model2DRigidLander::StateTransitionEquation(const leda_vector &x, const leda_vector &u)
{
  leda_vector dx(4);

  /* Transfer the velocity */
  dx[0] = x[2];
  dx[1] = x[3];
  dx[2] = 0.0;
  if (u[0] == 1) 
    dx[2] = Fs;
  if (u[0] == 3) 
    dx[2] = -Fs;
  dx[3] = -Mass*G;
  if (u[0] == 2) 
    dx[3] += Fu;

  //cout << "x: " << x << "  Dx: " << dx << "  u: " << u[0] << "\n";

  return dx;
}





double Model2DRigidLander::Metric(const leda_vector &x1, const leda_vector &x2) {
  double d = 0.0;
  int i;

  for (i = 0; i < 4; i++) {
    d += sqrt(sqr(x1[i] - x2[i]) / (UpperState[i] - LowerState[i]));
  }
  
  //cout << "x1: " << x1 << "  x2: " << x2 << "   Metric: " << d << "\n";

  return d;
}

// Model3D by Mikael Beckius 2000-04-20.

Model3D::Model3D(leda_string path):Model(path){
  leda_vector v;
  double theta;

  StateDim = 4;
  InputDim = 4;

  LowerState = leda_vector(4);
  UpperState = leda_vector(4);
  
  if (is_file(FilePath+"LowerState"))
    ReadLowerState();
  else {
     LowerState[0] = -70.0; LowerState[1] = 0.0; LowerState[2] = -70.0; 
     LowerState[3] = 0.0; 
  }
  
  if (is_file(FilePath+"UpperState"))
    ReadUpperState();
  else {
    UpperState[0] = 70.0; UpperState[1] = 40.0; UpperState[2] = 70.0; 
    UpperState[3] = 2.0*PI;
  }
  
  // Make the list of Inputs
  if (is_file(FilePath+"Inputs"))
    ReadInputs();
  else {
    for (theta = 0.0; theta < 2.0*PI; theta += 2.0*PI/NUM_INPUTS_2DRIGID) {
      //v = leda_vector(4); v[0] = 0;v[1] = 0;v[2] = 0;v[3] = -0.2;
      //Inputs.push_back(v);
      //v = leda_vector(4); v[0] = 0;v[1] = 0;v[2] = 0;v[3] = 0.2;
      //Inputs.push_back(v);
      v = leda_vector(4); v[0] = cos(theta);v[1] = sin(theta);v[2] = 0;v[3] = -0.2;
      Inputs.push_back(v);
      v = leda_vector(4); v[0] = cos(theta);v[1] = sin(theta);v[2] = 0;v[3] = 0.2;
      Inputs.push_back(v);
      v = leda_vector(4); v[0] = cos(theta);v[1] = sin(theta);v[2] = -0.3;v[3] = 0.0;
      Inputs.push_back(v);
      v = leda_vector(4); v[0] = cos(theta);v[1] = sin(theta);v[2] = 0.3;v[3] = 0.0;
      Inputs.push_back(v);
     }  
    v = leda_vector(4); v[0] = 0;v[1] = 0;v[2] = 0;v[3] = -0.2;
    Inputs.push_back(v);
    v = leda_vector(4); v[0] = 0;v[1] = 0;v[2] = 0;v[3] = 0.2;
    Inputs.push_back(v);
    v = leda_vector(4); v[0] = 0;v[1] = 0;v[2] = -0.3;v[3] = 0;
    Inputs.push_back(v);
    v = leda_vector(4); v[0] = 0;v[1] = 0;v[2] = 0.3;v[3] = 0;
    Inputs.push_back(v);
    //Inputs.push_back(leda_vector(cos(theta),sin(theta),0.0));
    //Inputs.push_back(leda_vector(cos(theta),sin(theta),-0.1));
    //Inputs.push_back(leda_vector(cos(theta),sin(theta),0.1));
    //}
    //Inputs.push_back(leda_vector(0.0,0.0,0.0,-0.1));
    //Inputs.push_back(leda_vector(0.0,0.0,0.0,0.1));
  }
}


leda_vector Model3D::StateTransitionEquation(const leda_vector &x, const leda_vector &u){
  leda_vector dx(4);
  dx = Speed*u;
  return dx;
}

double Model3D::Metric(const leda_vector &x1, const leda_vector &x2){
  double fd = fabs(x1[3]-x2[3]);
  double dtheta = min(fd,2.0*PI - fd);
  return sqrt(sqr(x1[0] - x2[0]) + sqr(x1[1] - x2[1]) + sqr(x1[2] - x2[2]) + sqr(50.0/PI*dtheta));
}


// Handle S^1 topology properly (for rotation)
leda_vector Model3D::LinearInterpolate(const leda_vector &x1, const leda_vector &x2, 
				const double &a) {

  leda_vector v;

  v = a*x1 + (1.0-a)*x2;

  if (x2[3] - x1[3] > PI) {
    if (x1[3] > x2[3])
      v[3] = a*x1[3] + (1.0-a)*(x2[3]+PI);
    else
      v[3] = a*(x1[3]+PI) + (1.0-a)*x2[3];
  }

  if (v[3] > 2.0*PI)
    v[3] -= 2.0*PI;

  return v;
}


leda_vector Model3D::Integrate(const leda_vector &x, const leda_vector &u, const double &h)
{
  leda_vector nx(4);

  nx = RungeKuttaIntegrate(x,u,h);

  // Make sure the S^1 topology is preserved for 2D rotation
  if (nx[3] > 2.0*PI)
    nx[3] -= 2.0*PI;
  if (nx[3] < 0.0)
    nx[3] += 2.0*PI;

  return nx;
}

// Constructor
Model3Dx::Model3Dx(leda_string path = ""):Model(path) {
  double theta;
  Speed = 1.0;
  StateDim = 3;
  InputDim = 3;

  if (is_file(FilePath+"LowerState"))
    ReadLowerState();
  else {
    LowerState = leda_vector(-70.0,0.0,-70.0);
  }

  if (is_file(FilePath+"UpperState"))
    ReadUpperState();
  else {
    UpperState = leda_vector(70.0,40.0,70.0);
  }

  // Make the list of Inputs
  if (is_file(FilePath+"Inputs"))
    ReadInputs();
  else {
    for (theta = 0.0; theta < 2.0*PI; theta += 2.0*PI/NUM_INPUTS_2DRIGID) {
    //for (theta = -0.5*PI; theta < 0.5*PI; theta += PI/NUM_INPUTS_2DRIGID) {
      //Inputs.push_back(leda_vector(cos(theta),sin(theta),0.0));
      Inputs.push_back(leda_vector(cos(theta),sin(theta),-0.2));
      Inputs.push_back(leda_vector(cos(theta),sin(theta),0.2));
    }
    //Inputs.push_back(leda_vector(0.0,0.0,-0.3));
    //Inputs.push_back(leda_vector(0.0,0.0,0.3));
  }

}


leda_vector Model3Dx::StateTransitionEquation(const leda_vector &x, const leda_vector &u) {

  leda_vector dx(3);

  dx = Speed*u;
  return dx;
}



double Model3Dx::Metric(const leda_vector &x1, const leda_vector &x2) {

  double fd = fabs(x1[2]-x2[2]);
  double dtheta = min(fd,2.0*PI - fd);

  //return sqrt(sqr(x1[0] - x2[0]) + sqr(x1[1] - x2[1]) + sqr(50.0/PI*dtheta));
  double rho;
  //return sqrt(sqr(x1[0] - x2[0]) + sqr(x1[1] - x2[1]));
  rho = (x1 - x2).length();
  
  return rho;
}



// Handle S^1 topology properly (for rotation)
leda_vector Model3Dx::LinearInterpolate(const leda_vector &x1, const leda_vector &x2, 
				const double &a) {

  leda_vector v;

  v = a*x1 + (1.0-a)*x2;

  if (x2[2] - x1[2] > PI) {
    if (x1[2] > x2[2])
      v[2] = a*x1[2] + (1.0-a)*(x2[2]+PI);
    else
      v[2] = a*(x1[2]+PI) + (1.0-a)*x2[2];
  }

  if (v[2] > 2.0*PI)
    v[2] -= 2.0*PI;

  cout<<"Model2DRigid::LinearInterpolate"<<endl;
  return v;
}

leda_vector Model3Dx::Integrate(const leda_vector &x, const leda_vector &u, const double &h)
{
  leda_vector nx(3);

  nx = RungeKuttaIntegrate(x,u,h);

  // Make sure the S^1 topology is preserved for 2D rotation
  //if (nx[3] > 2.0*PI)
  // nx[3] -= 2.0*PI;
  //if (nx[3] < 0.0)
  //nx[3] += 2.0*PI;

  return nx;
}



//////////////////////////////////////
// Constructor
////////////////////////////////////

Model3Dy::Model3Dy(leda_string path = ""):Model(path) {
  double theta;
  Speed = 2.0;
  StateDim = 4;
  InputDim = 4;
  leda_vector v;
  
  LowerState = leda_vector(4);
  UpperState = leda_vector(4);

  if (is_file(FilePath+"LowerState"))
    ReadLowerState();
  else {
    LowerState[0] = -70.0; LowerState[1] = 0.0; LowerState[2] = -70.0; 
    LowerState[3] = 0.0; 
    //LowerState = leda_vector(-70.0,0.0,-70.0);
  }

  if (is_file(FilePath+"UpperState"))
    ReadUpperState();
  else {
    UpperState[0] = 70.0; UpperState[1] = 40.0; UpperState[2] = 70.0; 
    UpperState[3] = 2.0*PI;
    //UpperState = leda_vector(70.0,40.0,70.0);
  }

  // Make the list of Inputs
  if (is_file(FilePath+"Inputs"))
    ReadInputs();
  else {
    for (theta = 0.0; theta < 2.0*PI; theta += 2.0*PI/NUM_INPUTS_3D) {
    //for (theta = -0.5*PI; theta < 0.5*PI; theta += PI/NUM_INPUTS_3D_FORWARD) {
      //Inputs.push_back(leda_vector(cos(theta),sin(theta),0.0));
      //Inputs.push_back(leda_vector(cos(theta),sin(theta),-0.2));
      //Inputs.push_back(leda_vector(cos(theta),sin(theta),0.2));
      //v = leda_vector(4); v[0] = cos(theta);v[1] = 0;v[2] = sin(theta); v[3] = theta;
      //Inputs.push_back(v);
      //v = leda_vector(4); v[0] = cos(theta);v[1] = 0;v[2] = sin(theta); v[3] = -theta;
      //Inputs.push_back(v);
      //v = leda_vector(4); v[0] = cos(theta);v[1] = 0.3;v[2] = sin(theta); v[3] = theta;
      //Inputs.push_back(v);
      //v = leda_vector(4); v[0] = cos(theta);v[1] = -0.3;v[2] = sin(theta); v[3] = -theta;
      //Inputs.push_back(v);
      v = leda_vector(4); v[0] = cos(theta);v[1] = 0;v[2] = sin(theta); v[3] = 0;
      Inputs.push_back(v);
      //v = leda_vector(4); v[0] = cos(theta);v[1] = 0;v[2] = sin(theta); v[3] = 0;
      //Inputs.push_back(v);
    }  
    v = leda_vector(4); v[0] = 0;v[1] = 0;v[2] = 0; v[3] = -0.4;
    Inputs.push_back(v);
    v = leda_vector(4); v[0] = 0;v[1] = 0;v[2] = 0; v[3] = 0.4;
    Inputs.push_back(v);
    v = leda_vector(4); v[0] = 0;v[1] = -0.4;v[2] = 0.0; v[3] = 0;
    Inputs.push_back(v);
    v = leda_vector(4); v[0] = 0;v[1] = 0.4;v[2] = 0.0; v[3] = 0;
    Inputs.push_back(v);
    //Inputs.push_back(leda_vector(0.0,0.0,-0.3));
    //Inputs.push_back(leda_vector(0.0,0.0,0.3));
   }

}


leda_vector Model3Dy::StateTransitionEquation(const leda_vector &x, const leda_vector &u) {

  leda_vector dx(4);

  dx[0] = Speed*u[0];
  dx[1] = Speed*u[1]*1.5;
  dx[2] = Speed*u[2];
  dx[3] = u[3];
  return dx;
}



double Model3Dy::Metric(const leda_vector &x1, const leda_vector &x2) {
  double fd = fabs(x1[3]-x2[3]);
  double dtheta = min(fd,2.0*PI - fd);
  return sqrt(sqr(x1[0] - x2[0]) + 1*sqr(x1[1] - x2[1]) + sqr(x1[2] - x2[2]) + sqr(30.0/PI*dtheta));

  //double fd = fabs(x1[2]-x2[2]);
  //double dtheta = min(fd,2.0*PI - fd);
  //return sqrt(sqr(x1[0] - x2[0]) + sqr(x1[1] - x2[1]) + sqr(50.0/PI*dtheta));
  //double rho;
  //return sqrt(sqr(x1[0] - x2[0]) + sqr(x1[1] - x2[1]));
  //rho = (x1 - x2).length();
  //return rho;
}



// Handle S^1 topology properly (for rotation)
leda_vector Model3Dy::LinearInterpolate(const leda_vector &x1, const leda_vector &x2, 
				const double &a) {

  leda_vector v;

  v = a*x1 + (1.0-a)*x2;

  if (x2[3] - x1[2] > PI) {
    if (x1[3] > x2[3])
      v[3] = a*x1[3] + (1.0-a)*(x2[3]+PI);
    else
      v[3] = a*(x1[3]+PI) + (1.0-a)*x2[3];
  }

  if (v[3] > 2.0*PI)
    v[3] -= 2.0*PI;

  cout<<"Model2DRigid::LinearInterpolate"<<endl;
  return v;
}

leda_vector Model3Dy::Integrate(const leda_vector &x, const leda_vector &u, const double &h)
{
  leda_vector nx(4);

  nx = RungeKuttaIntegrate(x,u,h);

  // Make sure the S^1 topology is preserved for 2D rotation
  if (nx[3] > 2.0*PI)
  nx[3] -= 2.0*PI;
  if (nx[3] < 0.0)
  nx[3] += 2.0*PI;

  return nx;
}