#ifndef _ROBOCUP_NAVIGATION_H_
#define _ROBOCUP_NAVIGATION_H_

#include "Motor.h"
#include "util.h"


class Navigation {
  private:
    Motor *motors[3];
    float motorPos[3];
    
    float speed;
    float direction;
    float spin;
    
    void update();
  public:
    Navigation(Motor *m1, float p1, Motor *m2, float p2, Motor *m3, float p3);
    
    void setSpeed(float speed) {this->speed = CLAMP(0.0, speed, 1.0); update();}
    float getSpeed() {return speed;}
    
    void setDirection(float direction) {this->direction = direction; update();}
    float getDirection() {return direction;}
    
    void setSpin(float spin) {this->spin = CLAMP(-1.0, spin, 1.0); update();}
    float getSpin() {return spin;}
};

#endif