//#define DEBUG_MOVE_STEP
class FRobot {
  public:
    FRobot() {
      
      delay_time = 20;
    }
    void home() {}
    
    int read(int i) {
      return servo[i].read();
    }
    void set_delay(int t) {
      delay_time = t;
    }
    int move_step(int which, int target) {
      int pos;
      pos = servo[which].read();
      #ifdef DEBUG_MOVE_STEP
      Serial.print(which, DEC); Serial.print(" Target"); Serial.print(target,DEC);
      Serial.print("cur pos"); Serial.println(pos,DEC);
      #endif
      if (pos < target)
        servo[which].write(pos + 1);
      else if (pos > target)
        servo[which].write(pos - 1);
      else if (pos == target)
        return 1;
      return 0;
    }
    void motion(unsigned char m[][4], int steps)
    {
      int i, j, ret = 0;;
      for (i = 0; i < steps; i++) {
        Serial.print("motion: ");
        Serial.println(i, DEC);
        do {
          ret = 0;
          for (j = 0; j < 4; j++)  ret += move_step(j, m[i][j]);
          delay(delay_time);
        } while (ret < 4);
      }
      Serial.println("motion end");
    }
    void Execute(int val[])
    {
      int j, ret;
      do {
        ret = 0;
        for (j = 0; j < 4; j++)  ret += move_step(j, val[j]);
        delay(delay_time);
      } while (ret < 4);
      Serial.print("move to "); Serial.print(val[0]); Serial.print(val[1]); Serial.print(val[2]); Serial.println(val[3]);
    }

  public:
    Servo *servo;
    int delay_time;
};

class FRobot_Arm : public FRobot {
  public:
    FRobot_Arm(): FRobot() {
      servo = s;
      
    }
    void attach(int p1, int p2, int p3, int p4){
      servo[0].attach(p1);
      servo[1].attach(p2);
      servo[2].attach(p3);
      servo[3].attach(p4);
    }
    void home(int a, int b, int c, int d) {
      servo[0].write(a);
      servo[1].write(b);
      servo[2].write(c);
      servo[3].write(d);
    }
  private:
    Servo s[4];

};