ControlSpace.h

#ifndef PLANNING_CONTROL_SPACE_H
#define PLANNING_CONTROL_SPACE_H

#include "CSpace.h"
#include "Interpolator.h"
#include "CSpaceHelpers.h"
namespace Math {
  class VectorFieldFunction;
} //namespace Math
typedef Vector State;
typedef Vector ControlInput;
class KinodynamicMilestonePath;

//forward declarations
class ControlSpace;
class ReversibleControlSpace;
class SteeringFunction;
class IntegratedControlSet;
class IntegratedControlSpace;
class KinematicControlSpace;


class ControlSpace
{
public:
  ControlSpace() {}
  virtual ~ControlSpace() {}

  virtual std::string VariableName(int i);

  virtual std::shared_ptr<CSet> GetControlSet(const State& x) { return myControlSet; }

  virtual std::shared_ptr<SteeringFunction> GetSteeringFunction() { return mySteeringFunction; }

  virtual InterpolatorPtr Simulate(const State& x0, const ControlInput& u)=0;

  virtual void Successor(const State& x0, const ControlInput& u,State& x1) {
    InterpolatorPtr p = Simulate(x0,u);
    x1 = p->End();
  }

  virtual Math::VectorFieldFunction* SuccessorNumeric() { return NULL; }

  virtual void SampleControl(const State& x,ControlInput& u) { GetControlSet(x)->Sample(u); }

  virtual bool IsValidControl(const State& x,const ControlInput& u) { return GetControlSet(x)->Contains(u); }

  std::shared_ptr<CSet> myControlSet;
  std::shared_ptr<SteeringFunction> mySteeringFunction;
};

class ReversibleControlSpace : public ControlSpace
{
public:
  virtual bool ReverseControl(const State& x0,const State& x1,ControlInput& u) { return false; }

  std::shared_ptr<ControlSpace> reverseControlSpace;
};

class SteeringFunction
{
public:
  SteeringFunction() {}
  virtual ~SteeringFunction() {}
  virtual bool IsExact() const { return true; };
  virtual bool IsOptimal() const { return true; };
  virtual bool Connect(const State& x,const State& y,KinodynamicMilestonePath& path)=0;
};


 /*
class MultiControlSpace : public ControlSpace
{
public:
  MultiControlSpace();
  MultiControlSpace(const std::vector<int>& istateStarts,const std::vector<std::shared_ptr<ControlSpace> >& spaces);
  MultiControlSpace(const MultiCSpace* space,const std::vector<std::shared_ptr<ControlSpace> >& spaces);
  void Add(int istatemin,int istatemax,const std::shared_ptr<ControlSpace>& item);
  void Add(CSpace* space,const std::shared_ptr<ControlSpace>& item);
  virtual InterpolatorPtr Simulate(const State& x0, const ControlInput& u);
  virtual void Successor(const State& x0, const ControlInput& u,State& x1);
  virtual Math::VectorFieldFunction* SuccessorNumeric();

  std::vector<pair<int,int>  istateRanges;
  std::vector<std::shared_ptr<ControlSpace> > components;
};
*/

class IntegratedControlSet : public CSet
{
public:
  enum TimeSelection { Uniform, Maximum, Biased };

  IntegratedControlSet(const std::shared_ptr<CSet>& base,Real dtmax);
  virtual int NumDimensions() const;
  virtual bool Project(Config& x);
  virtual bool IsSampleable() const;
  virtual void Sample(ControlInput& u);
  virtual bool Contains(const ControlInput& u);
  
  TimeSelection timeSelection;
  std::shared_ptr<CSet> base;
  Real dtmax;
};

class IntegratedControlSpace : public ControlSpace
{
 public:
  typedef void (*DynamicsFn)(const State& x, const ControlInput& u,State& dx);

  enum Integrator { Euler, RK4 };

  IntegratedControlSpace(const std::shared_ptr<CSet>& fControlSet,Real dt=0.01,Real dtmax=0.1);
  IntegratedControlSpace(DynamicsFn f,const std::shared_ptr<CSet>& fControlSet,Real dt=0.01,Real dtmax=0.1);
  void SetGeodesicSpace(GeodesicSpace* space);
  void SetBaseControlSet(std::shared_ptr<CSet> baseControlSet);
  std::shared_ptr<CSet> GetBaseControlSet();
  virtual std::string VariableName(int i);
  virtual InterpolatorPtr Simulate(const State& x0, const ControlInput& u);
  virtual std::shared_ptr<CSet> GetControlSet(const Config& x);

  //subclasses may override the following:

  virtual void Derivative(const State& x, const ControlInput& u,State& dx);

  virtual void UpdateIntegrationParameters(const State& x) {}

  DynamicsFn myDynamics;
  Integrator type;
  GeodesicSpace* space;
  std::shared_ptr<CSet> controlSet;
  Real dt;          
  Real dtmax;       
};



class KinematicControlSpace : public ReversibleControlSpace
{
 public:
  KinematicControlSpace(const std::shared_ptr<CSpace>& base,Real maxNeighborhoodRadius=0.1);
  virtual ~KinematicControlSpace() {}

  virtual std::string VariableName(int i);
  virtual std::shared_ptr<CSet> GetControlSet(const Config& x);

  virtual InterpolatorPtr Simulate(const State& x0, const ControlInput& u);
  virtual void Successor(const State& x0, const ControlInput& u,State& x1);
  virtual Math::VectorFieldFunction* SuccessorNumeric();

  virtual bool ReverseControl(const State& x0,const State& x1,ControlInput& u);

  std::shared_ptr<CSpace> base;
  Real maxNeighborhoodRadius;
};



#endif