/** CoupledPendulums.h
 * Implementation of the coupled inverted pendulums benchmark.
 *
 *  @date: 30.11.2010
 *  @author: Heiko Hamann
*/

#ifndef COUPLEDPENDULUMS_H_
#define COUPLEDPENDULUMS_H_



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


#define PI 3.14159265



class CoupledPendulums {
  
 public:
  CoupledPendulums();
  CoupledPendulums(int myCrabNum);
  virtual ~CoupledPendulums();
  int performOneStep(std::vector< std::vector<int> > motorControl);
  std::vector< std::vector<int> > getSensorValues();
  std::vector< std::vector<float> > getState();
  float getFitness();
  void initialPosition();
  void writeTrajectoryToFile();
  

 private:
  float motor(float u, float v);
  float changeAngularVel(float phi, float omega, float u, float v);
  int checkForBorders();
  void logTrajectory();

 private:
  float Kp;
  float Kl;
  float g;
  float length;
  float Vmotor;
  float Vbreak;
  float deltaUV;

  float deltaT;

  float chainMin;
  float chainMax;
  float worldWidth;

  int stepsPerRun;
  int currentStep;

  unsigned int crabNum;

  std::vector<float> phi;
  std::vector<float> omega;
  std::vector<float> v;
  std::vector<float> u;
  std::vector<float> x;

  float fitness;
  
};



#endif /* COUPLEDPENDULUMS_H_ */