//////////////////////////////
// Engine code
//////////////////////////////
#define DEG2RAD PI / 180.0f
#define RAD2DEG 180.0f / PI
#define ANGLE_PER_STEP 1.8f

class Engine {
  private:
    byte stepPin;
    byte directionPin;
    float angle;

    void init() {
      angle = 0;
      pinMode(stepPin, OUTPUT);
      digitalWrite(stepPin, LOW);
      pinMode(directionPin, OUTPUT);
      digitalWrite(directionPin, HIGH);
    }

  public:
    Engine(byte stepPin, byte directionPin) {
      // Use 'this->' to make the difference between the
      // 'pin' attribute of the class and the
      // local variable 'pin' created from the parameter.
      this->stepPin = stepPin;
      this->directionPin = directionPin;
      init();
    }

    void turnTo(float newAngle) {
      float angleDelta = angle - newAngle;
      if (angleDelta > 180.0f) {
        angleDelta -= 360.0f;
      }
      if (angleDelta < -180.0f) {
        angleDelta += 360.0f;
      }
      if (angleDelta < 1.0f && angleDelta > -1.0f) {
        return;
      }

      digitalWrite(directionPin, angleDelta > 0.0f ? LOW : HIGH);
      do {
        digitalWrite(stepPin, HIGH);
        digitalWrite(stepPin, LOW);
        if (angleDelta > 0) {
          angle -= ANGLE_PER_STEP;
          angleDelta -= ANGLE_PER_STEP;
        } else {
          angle += ANGLE_PER_STEP;
          angleDelta += ANGLE_PER_STEP;
        }
        if (angle >= 360.0f) {
          angle -= 360.0f;
        }
        if (angle < 0.0f) {
          angle += 360.0f;
        }
      } while (angleDelta > 1.0f || angleDelta < -1.0f);
    }
};

//////////////////////////////
// Joystick code
//////////////////////////////
class Joystick {
  private:
    byte verticalPin;
    byte horizontalPin;
    byte selectPin;

    int getVerticalValue() {
      return map(analogRead(verticalPin), 0, 1023, 100, -100);
    }

    int getHorizontalValue() {
      return map(analogRead(horizontalPin), 0, 1023, 100, -100);
    }

    void init() {
      pinMode(verticalPin, INPUT);
      pinMode(horizontalPin, INPUT);
      pinMode(selectPin, INPUT_PULLUP);
    }

  public:
    Joystick(byte verticalPin, byte horizontalPin, byte selectPin) {
      this->verticalPin = verticalPin;
      this->horizontalPin = horizontalPin;
      this->selectPin = selectPin;
    }

    bool hasInput() {
      int vertical = getVerticalValue();
      int horizontal = getHorizontalValue();

      return vertical < -1 || vertical > 1 || horizontal < -1 || horizontal > 1;
    }

    float getAngle() {
      int vertical = getVerticalValue();
      int horizontal = getHorizontalValue();
      if (vertical == 0 && horizontal == 0) {
        return;
      }

      float angle = atan2(vertical, horizontal) * RAD2DEG + 90;
      if (angle < 0) {
        angle += 360;
      } if (angle >= 360) {
        angle -= 360;
      }

      return angle;
    }
};

Joystick joystick1(A0, A1, 0);
Engine engine1(22, 23);
#include <Servo.h>

Servo arm; // Create a "Servo" object called "arm"

void setup() {
  Serial.begin(9600);
  arm.attach(2);
}

void loop() {
  while (Serial.available() > 0) {
    if (joystick1.hasInput()) {
      engine1.turnTo(joystick1.getAngle());
      int angle = joystick1.getAngle() + 90;
      if (angle > 181) {
        angle %= 180;
      }
      arm.write(angle);
      delay(5);
    }
  }
}