#include <Stepper.h>
#include <vector>
#include <Arduino.h>

using namespace std;

int sairloop = 0;

int buttonUpState;
int buttonSelectState;
int buttonDownState;

int GLOBAL_SPEED = 90;
const int GLOBAL_STEPS = 200;
const int GLOBAL_TARGET = 50;

class Engine
{
  private:
    int AP, AN, BP, BN;
    char CORD;
    int target;
    int position;

  public:
    Stepper *stepperConstructor;

    Engine(char _cord, int _bn, int _bp, int _ap, int _an) : AP(_ap), AN(_an), BP(_bp), BN(_bn), CORD(_cord) {
      this->stepperConstructor = new Stepper(GLOBAL_STEPS, _bn, _bp, _ap, _an);
      this->stepperConstructor->setSpeed(GLOBAL_SPEED);

      this->target = GLOBAL_TARGET;

      this->position = 0;
    }

    char getCord() {
      return this->CORD;
    }

    int getTarget() {
      return this->target;
    }

    void reverseTarget() {
      this->target *= (-1);
    }

    void runToTarget() {
      if (this->target > 0) {
        this->stepperConstructor->step(1);
        this->position++;
      }
      else {
        this->stepperConstructor->step(-1);
        this->position--;
      }
    }

    int getPosition() {
      return this->position;
    }

    void oneStep(int signal_)
    {
      this->stepperConstructor->step(signal_ * 1);
      this->position = 0;
    }
};

enum Direction
{
  UP = 0,
  DOWN = 1
};

class EnginesSet
{
  private:
    std::vector<Engine *> engines;
    std::vector<Engine *> enginesToPlay;


  public:
    EnginesSet() {}

    void insertMotor(char cord, int BN, int BP, int AP, int AN) {
      Engine *engineConstrutor = new Engine(cord, BN, BP, AP, AN);

      this->engines.push_back(engineConstrutor);
    }

    void boot(); // Lógica de fim de curso

    void addToenginesToPlay(std::string cords) {
      for (auto &cord : cords)
      {
        for (auto engine : this->engines)
        {
          if (engine->getCord() == cord)
          {
            if (buttonUpState == 0 || buttonSelectState == 0 || buttonDownState == 0) {
              sairloop = 1;
            }
            this->enginesToPlay.push_back(engine);
          }
        }
      }
    }

    void playMany(int times = 1) {
      Engine* anyEngine = this->enginesToPlay[0];

      for (int i = 0; i < times; i++) {
        if (anyEngine->getTarget() > 0) {
          while (anyEngine->getPosition() < anyEngine->getTarget()) {
            for (auto engine : this->enginesToPlay) {
              if (buttonUpState == 0 || buttonSelectState == 0 || buttonDownState == 0) {
                sairloop = 1;
              }
              engine->runToTarget();
              delay(3);
            }
          }
        } else {
          while (anyEngine->getPosition() > anyEngine->getTarget()) {
            for (auto engine : this->enginesToPlay) {
              if (buttonUpState == 0 || buttonSelectState == 0 || buttonDownState == 0) {
                sairloop = 1;
              }
              engine->runToTarget();
              delay(3);
            }
          }
        }

        for (auto engine : this->enginesToPlay) {
          engine->reverseTarget();
        }

      }

      this->enginesToPlay.clear();
    }

    void parseFile(string stream) {
      char cordaChar;
      string subStream = "";
      int forControl = 0;
      int strControl = 0;
      for ( forControl; forControl < stream.size(); forControl++) {
        cordaChar = stream[forControl];
        if ( cordaChar == ' ') {
          if (stream[forControl - 1] == 'd') {
            runThrough(DOWN);
          }
          else if (stream[forControl - 1] == 's') {
            runThrough(UP);
          }
          else {
            for (strControl; strControl < forControl; strControl++) {
              subStream = subStream + stream[strControl];
            }
            this->addToenginesToPlay(subStream);
            Serial.println(subStream.c_str());
            this->playMany();
            subStream = "";
          }
          delay(500);
          strControl = forControl + 1;
        }
      }
    }

    void runThrough(Direction direction) {

      if (direction == UP) {
        for (auto it = this->engines.begin() ; it != this->engines.end() ; ++it) {
          this->enginesToPlay.push_back(*it);
          delay(100);
          this->playMany();
        }
      } else {
        for (auto it = this->engines.rbegin() ; it != this->engines.rend() ; ++it) {
          this->enginesToPlay.push_back(*it);
          delay(100);
          this->playMany();
        }
      }

    }

    void tune(int tunePosition) {
      switch (tunePosition) {
        case 0:
          {
            parseFile("E ");
            break;
          }
        case 1:
          {
            parseFile("A ");
            break;
          }
        case 2:
          {
            parseFile("D ");
            break;
          }
        case 3:
          {
            parseFile("B ");
            break;
          }
        case 4:
          {
            parseFile("G ");
            break;
          }
        case 5:
          {
            parseFile("e ");
            break;
          }
      }
    }

    void playOneStep(int cordReset, int signal)
    {
      this->engines[cordReset]->oneStep(signal);
    }

};

EnginesSet violao;
Engine cordA = Engine('A', 13, 12, 14, 27);
Engine cordB = Engine('B', 18, 19, 21, 22);
Engine cordE = Engine('E', 15, 2, 4, 5);

void setup()
{
  // set the speed at 60 rpm:
  violao.insertMotor('A', 13, 12, 14, 27);
  violao.insertMotor('E', 15, 2, 4, 5);
  violao.insertMotor('B', 18, 19, 21, 22);

  // initialize the serial port:
  Serial.begin(9600);
}

void loop()
{
  if (cordA.getTarget() > 0) {
    while (cordA.getPosition() < cordA.getTarget()) {
      cordA.runToTarget();
    }
  }
  else {
    while (cordA.getPosition() > cordA.getTarget()) {
      cordA.runToTarget();
    }
  }

  cordA.reverseTarget();
  delay(500);

}