class GeksaFoot {
private:
// Положение центра локальной системы координат лапы относительно центра робота
  Vector3D m_p0;
// Углы поворота осей локальной системы координат лапы относительно центра робота
  Vector3D m_r0;
// объекты Multiservo, для непосредственного управления сервоприводами
  Multiservo m_coxaServo;   // сервопривод плеча
  Multiservo m_femoraServo; // сервопривод бедра
  Multiservo m_tibiaServo;  // сервопривод голени
public:
  GeksaFoot(Vector3D p0,Vector3D r0);
// выполняет активацию сервоприводов
  void begin(int coxaPin, int femoraPin, int tibiaPin);
// выполняет дезактивацию сервоприводов
  void end();   

// интерфейс управления лапой

  void coxaAngle(int);      // повернуть плечо на заданный угол (-90 .. 90 градусов)
  int coxaAngle();          // возвращает текущий угол поворота плеча

  void femoraAngle(int);    // наклонить бедро на заданный угол  (-90 .. 90 градусов)
  int femoraAngle();        // возвращает текущий угол наклона бедра

  void tibiaAngle(int);     // наклонить голень на заданный угол  (-90 .. 90 градусов)
  int tibiaAngle();         // возвращает текущий угол наклона голени

// вспомогательные методы для расчетов прямой и инверсной кинематики 

  // вычисляет углы поворотов ноги для достижения конечностью заданной точки
  int getAngles(Vector3D p, int& coxaAngle, int& femoraAngle, int& tibiaAngle);
  // вычисляет координату кончика лапы по заданным углам
  int getPoint(int coxaAngle, int femoraAngle, int tibiaAngle, Vector3D& p);
};

class Geksapod: public AJobManager {
  friend class MotionJob;
  friend CommandProcessorJob;
  // конечности робота
  GeksaFoot m_LeftFrontFoot;
  GeksaFoot m_LeftMidleFoot;
  GeksaFoot m_LeftBackFoot;
  GeksaFoot m_RigthFrontFoot;
  GeksaFoot m_RigthMidleFoot;
  GeksaFoot m_RigthBackFoot;
  // задача, отвечающая за движение конечностей
  MotionJob m_MotionJob;
private:
  // устанавливает конечности робота в заданное положение 
  // с предварительной проверкой на предельную допустимость углов
  // и корректность взаимного расположения конечностей
  int _setPose(int idx, int ca, int fa, int ta);
  int _setPose(int[FOOTS_COUNT][3]);  
  int _setPose(Vector3D points[FOOTS_COUNT]); 
protected:
  // перемещение конечностей робота в заданное положение в течение указанного времени
  int setPose(int idx, int ca, int fa, int ta, int actionTime); 
  int setPose(int pose[FOOTS_COUNT][3], int actionTime);
  int setPose(int idx, Vector3D p, int actionTime);
  int setPose(Vector3D points[FOOTS_COUNT], int actionTime = 0);
  int setPose(int ca, int fa, int ta, int actionTime);
  // текущее положение конечностей робота  
  void getPose(int idx, int& ca, int& fa, int& ta);
  void getPose(int pose[FOOTS_COUNT][3]);
  void getPose(int idx, Vector3D& p);
  void getPose(Vector3D points[FOOTS_COUNT]);
  // запускает выполнение указанного движения
  int execute(Motion* pMotion);
public:
  Geksapod();
  void setup();
  // основные управляющие команды
  int move(int speed, int time);    // движение по прямой
  int rotate(int speed, int time);  // поворот на месте
  void stop();                      // остановить движение
  // вспомогательные методы для расчетов прямой и инверсной кинематики 
  int getAngles(int idx, Vector3D p, int& ca, int& fa, int& ta);
  int getPoint(int idx, int coxaAngle, int femoraAngle, int tibiaAngle, Vector3D& p);
  int getAngles(Vector3D points[FOOTS_COUNT], int pose[FOOTS_COUNT][3]);
  int getPoints(int pose[FOOTS_COUNT][3], Vector3D points[FOOTS_COUNT]);
};

class AJob {
  friend class AJobManager;
private:
  AJobManager* m_pAJobManager;   
  AJob* mJobNext;                      
  unsigned long m_counter;         // Счетчик вызовов метода onRun
  unsigned long m_previousMillis; // метка времени предыдущего вызова onRun
  unsigned long m_currentMillis;  // текущая метка времени
  unsigned long m_delayMillis;    // интервал вызова метода onRun
  void run();
public:
  AJob(AJobManager*, unsigned long delay = 0L);
  ~AJob();
                  
  void finish();  // принудительное завершение задачи  
  long counter(); // возвращает количество вызовов onRun с момента старта задачи
  long setDelay(unsigned long); // устанавливает интервал вызова onRun
  unsigned long previousMillis();// возвращает метку времени предыдущего вызова onRun
  unsigned long currentMillis(); // возвращает текущую метку времени
                              
  virtual void onInit();  // вызывается один раз для инициализации задачи
  virtual void onRun();   // вызывается многократно в процессе работы
  virtual void onDone();  // вызывается при завершении задачи с помощью метода finish
};

class AJobManager {
  friend class AJob;
  AJob* mJobFirst;  // указатель на первый элемент списка задач
  void attach(AJob*);   // добавить задачу в список
  void dettach(AJob*); // удалить задачу из списка
  void dettachAll();   // удалить все задачи
public:  
  AJobManager();
  ~AJobManager();
  void setup();
  void loop();
};

class Motion {
  friend class MotionJob;
protected:
  long m_MaxTime;     // время выполнения движения в одном цикле
  long m_TotalTime;   // время отведенное на выполнение движения 
  bool m_IsLooped;    // признак цикличности движения
  Motion* m_pNext;    // указатель на следующий  движения
public:  
  Motion(long maxTime, bool isLooped, long totalTime = -1, Motion* pNext = NULL);
  ~Motion();
  
  inline long maxTime() { return m_MaxTime; }
  inline long totalTime() { return m_TotalTime; }
  inline bool isLooped() { return m_IsLooped; }

  // хотя бы один из этих методов должен иметь полноценную реализацию

  // возвращает координаты конечностей на момент времени 0 <= time <= m_MaxTime 
  virtual int getPose(long time, Vector3D points[FOOTS_COUNT]) { return E_NOT_IMPL; };  
  // возвращает углы поворота конечностей на момент времени 0 <= time <= m_MaxTime
  virtual int getPose(long time, int pose[FOOTS_COUNT][3]) { return E_NOT_IMPL; };       
};

class MotionJob: public AJob {
  enum STATUS { 
    NONE, RUNING, STOPING 
   } m_Status;
  
  Geksapod* m_pGeksapod;
  Motion* m_pMotion;
  long m_MotionTime;
  long m_TotalTime;
public:  
  MotionJob(Geksapod* pGeksapod);
  int execute(Motion* pMotion);
  void onRun();
};

class AJob {
  friend class AJobManager;
private:
  AJobManager* m_pAJobManager;   
  AJob* mJobNext;                      
  unsigned long m_counter;         // Счетчик вызовов метода onRun
  unsigned long m_previousMillis; // метка времени предыдущего вызова onRun
  unsigned long m_currentMillis;  // текущая метка времени
  unsigned long m_delayMillis;    // интервал вызова метода onRun
  void run();
protected:
  virtual void onInit();  // вызывается один раз для инициализации задачи
  virtual void onRun();   // вызывается многократно в процессе работы
  virtual void onDone();  // вызывается при завершении задачи с помощью метода finish
public:
  AJob(AJobManager*, unsigned long delay = 0L);
  ~AJob();

  void finish();  // принудительное завершение задачи  
  long counter(); // возвращает количество вызовов onRun с момента старта задачи
  long setDelay(unsigned long); // устанавливает интервал вызова onRun
  unsigned long previousMillis();// возвращает метку времени предыдущего вызова onRun
  unsigned long currentMillis(); // возвращает текущую метку времени
};