#include "arm4.h"

void motorSync(arm4_t * this) {
  stepMotorSync(&(this->motors));
}

Status_t arm4Init(arm4_t * this, char * port ) {
   stepMotorInit(&(this->motors));   
   stepMotorSetSerial(&(this->motors), port);
   stepMotorSpd(&(this->motors), 5000);
}

Status_t arm4Zero(arm4_t * this ) {
   
   serialStepper_t * robot;
   
   robot = &(this->motors);
   
  stepMotorInit(robot);
  stepMotorSetSerial(robot, "/dev/ttyS0");
  stepMotorSpd(robot, 4000);
  stepMotorQLimitUpdate(robot);  
  
  
  while(((robot->limitSwitches) & 0x01) == 0) {
    printf("Forward 1:\n");
    stepMotorStep(robot, 0, -1);
    stepMotorSync(robot);   
  }  
  while(((robot->limitSwitches) & 0x01) != 0) {
    printf("Back 1:\n");
    stepMotorStep(robot, 0, 1);
    stepMotorSync(robot);   
  }
  
  // these next two should be run at once.
  
  while(((robot->limitSwitches) & 0x02) == 0) {
    printf("Forward 2:\n");
    stepMotorStep(robot, 1, 1);
    stepMotorSync(robot);   
  }  
  while(((robot->limitSwitches) & 0x02) != 0) {
    printf("Back 2:\n");
    stepMotorStep(robot, 1, -1);
    stepMotorSync(robot);   
  }
  
       
   
  while(((robot->limitSwitches) & 0x04) != 0) {
    printf("Forward 3:\n");
    stepMotorStep(robot, 2, 1);
    stepMotorSync(robot);   
  }  
  while(((robot->limitSwitches) & 0x04) == 0) {
    printf("Back 3:\n");
    stepMotorStep(robot, 2, -1);
    stepMotorSync(robot);   
  }
  
   
     
  while(((robot->limitSwitches) & 0x08) != 0) {
    printf("Forward 4:\n");
    stepMotorStep(robot, 3, -1);
    stepMotorSync(robot);   
  }  
  while(((robot->limitSwitches) & 0x08) == 0) {
    printf("Back 4:\n");
    stepMotorStep(robot, 3, 1);
    stepMotorSync(robot);   
  }
       
   
  while(((robot->limitSwitches) & 0x10) != 0) {
    printf("Forward 5:\n");
    stepMotorStep(robot, 4, -1);
    stepMotorSync(robot);   
  }  
  while(((robot->limitSwitches) & 0x10) == 0) {
    printf("Back 5:\n");
    stepMotorStep(robot, 4, 1);
    stepMotorSync(robot);   
  }
  
}

Status_t M0F(arm4_t *this) { return stepMotorStep(&(this->motors), 0, 1); }
Status_t M0B(arm4_t *this) { return stepMotorStep(&(this->motors), 0, -1); }

Status_t M1F(arm4_t *this) { return stepMotorStep(&(this->motors), 1, 1); }
Status_t M1B(arm4_t *this) { return stepMotorStep(&(this->motors), 1, -1); }

Status_t M2F(arm4_t *this) { return stepMotorStep(&(this->motors), 2, 1); }
Status_t M2B(arm4_t *this) { return stepMotorStep(&(this->motors), 2, -1); }

Status_t M3F(arm4_t *this) { return stepMotorStep(&(this->motors), 3, 1); }
Status_t M3B(arm4_t *this) { return stepMotorStep(&(this->motors), 3, -1); }

Status_t M4F(arm4_t *this) { return stepMotorStep(&(this->motors), 4, 1); }
Status_t M4B(arm4_t *this) { return stepMotorStep(&(this->motors), 4, -1); }

Status_t M5F(arm4_t *this) { return stepMotorStep(&(this->motors), 5, 1); }
Status_t M5B(arm4_t *this) { return stepMotorStep(&(this->motors), 5, -1); }