// 3.6 2 amps


const int rangeSTick = 1023; // 0 is bottom
const int reangle = 45, angleRange = 90; // 0 is bottom
const float transformDesireToAngle = .09; // tranpoe
const int stillJoystick = 511;
const int deadband = 10;
const int motorPulseDelay = 500;

const float angleChange = 1.8;
float x = 0, y = 0; //angle of current postion
 
class MotorControl
{
  public:
  float currentPostion;
  float goPostion;
  int pastDesire = stillJoystick;
  bool reset;
  bool isButtonPressed;
  int button;
  int direct, pulse;
  
    MotorControl(int dire, int puls)
    {
      pinMode(dire,OUTPUT);
      pinMode(puls,OUTPUT);
      direct = dire;
      pulse = puls;
      currentPostion = reangle;
    }
    void whatToDo(int desire)
    {
      if (desire < 0)
      button = desire * -1;
      else
      goto yeet;
      switch (button)
      {
        default:
        goto yeet;
        case 1:
goto caseEnd;
        case 2:
goto caseEnd;
        case 3:
goto caseEnd;
        case 4:
goto caseEnd;
        case 5:
goto caseEnd;
        case 6:
goto caseEnd;
        case 7: 
      caseEnd:  
      button = 0;
      return; 
      }
      yeet:
      if(desire > (pastDesire + 10) || desire < (pastDesire - 10)) // noise from constant stick upload
      {
        goPostion = tranpose(desire);
        pastDesire = desire;
      }

      // two pins
      if(goPostion > (currentPostion + 5.0))
      {
        digitalWrite(direct, HIGH);
        currentPostion += angleChange;
        digitalWrite(pulse,HIGH); 
        delayMicroseconds(motorPulseDelay); 
        digitalWrite(pulse,LOW); 
        delayMicroseconds(motorPulseDelay);
      }
      return;
      if((currentPostion - 5.0) > goPostion)
      {
        digitalWrite(direct, LOW);
        currentPostion -= angleChange;
        digitalWrite(pulse,HIGH); 
        delayMicroseconds(motorPulseDelay); 
        digitalWrite(pulse,LOW); 
        delayMicroseconds(motorPulseDelay);
      }
    }
    float tranpose(int desire)
    {
      int temp = desire * transformDesireToAngle;
      return temp;
    }
    void calibrate()
    {
      currentPostion = reangle;
      goPostion = reangle;
    }
};


// defines pins numbers
const int stepPinM1 = 3; // motor 1
const int dirPinM1 = 5;
const int stepPinM2  = 6; // motor 2
const int dirPinM2 = 9;

MotorControl xMotor(stepPinM1, dirPinM1);
MotorControl yMotor(stepPinM2, dirPinM2);

void setup() {
  
}


void loop() {
  xMotor.whatToDo(700);// angle
  yMotor.whatToDo(700);// angle
}