/*

  Stepper Motor A4988 Driver Example

*/
#include <AccelStepper.h>
// Define a stepper and the pins it will use
AccelStepper stepper(AccelStepper::DRIVER, 6, 4);
#define DRV_DIR 2
#define DRV_STEP  9

class RailCrossing {
  public:
    RailCrossing(AccelStepper &stepper) {
      m_stp = &stepper;
    }
    void begin() {
        m_stp->setMaxSpeed(100.0);      // Set Max Speed of Stepper (Slower to get better accuracy)
        m_stp->setAcceleration(20.0);  // Set Acceleration of Stepper
    }

    void open() {
        if (m_state == 0) {
            m_stp->moveTo(0);
            m_state = -1;
        }
    }
    void close() {
        if (m_state == 1) {
            m_stp->moveTo(-50);
            m_state = -1;
        }
    }
    void run() {
        m_stp->run();
        if (m_stp->currentPosition() == -50) {
            m_state = 0;
        } else if (m_stp->currentPosition() == 0) {
            m_state = 1;
        } 
        
    }
    int16_t currentPos() {
        return m_stp->currentPosition() * 1.8;
    }
    int8_t state() {
        return m_state;
    }

    
  private:
    int16_t m_pos;
    int8_t m_state;
    uint32_t m_millis;
    AccelStepper *m_stp;
};

RailCrossing railc(stepper);


class StepperMotor {
  public:
  StepperMotor(AccelStepper &stepper) {
      m_stp = &stepper;
  }
  void write(uint16_t anglePos) {
      if (anglePos <= 180) {
          //byte step = (byte)(m_anglePos - anglePos) / 1.8;
          float stepAngle = (m_anglePos - anglePos) / 1.8;
          if ((byte)stepAngle >= 1) {
              Serial.println("moveTo");
              Serial.println(stepAngle);
              m_anglePos = anglePos;
              m_stp->moveTo((byte)stepAngle);
          }

          //m_anglePos = anglePos;
          Serial.println(m_anglePos);


      }

  }
  private:
    AccelStepper *m_stp;
    float m_stepAngle = 100;
    uint16_t m_anglePos = 180;

};

//StepperMotor stpm(stepper);

#define END_OPEN_SWITCH_PIN 6

long relativePos;

void goToRPos(long p) {
    long rpos = p - relativePos;
    //Serial.println(rpos);
    relativePos = p;
    stepper.move(rpos);

}

void setup() {
  Serial.begin(115200);  // Start the Serial monitor with speed of 9600 Bauds

  pinMode(END_OPEN_SWITCH_PIN, INPUT_PULLUP);

  delay(5);  // Wait for EasyDriver wake up

  //  Set Max Speed and Acceleration of each Steppers at startup for homing
  //stepper.setMaxSpeed(5000.0);      // Set Max Speed of Stepper (Slower to get better accuracy)
  //stepper.setAcceleration(1000.0);  // Set Acceleration of Stepper
  /*goToRPos(200);
  goToRPos(0);
  goToRPos(-200);
  goToRPos(0);*/
  //stpm.write(179);
  //stpm.write(178);
  //stpm.write(90);
  railc.begin();
  //railc.close();

  
}
long tp = 0;
long tp1 = 200;
byte g_state = 1;



int16_t currP;
int16_t currPold = 361;

void loop() {
  railc.run();
  if (railc.state() == 1) {
      railc.close();
      
  } else if (railc.state() == 0) {
      railc.open();
  } 

  currP = railc.currentPos();
  if (currP != currPold) {
      currPold = currP;
      Serial.println(currP);
  }
#if (0)
    switch (g_state) {
      case 0:
        /* move(long) muove dalla posizione relativa */
        //stepper.move(tp);
        //goToRPos(tp);
        /* moveTo(long) muove dalla posizione assoluta */
        stepper.moveTo(tp);
        
        g_state = 1;
        break;
      case 1:
        if (stepper.currentPosition() == 0 && tp != -400) {
            Serial.println("go 200;");
            tp = -400;
            g_state = 0;
        } else if (stepper.currentPosition() == -400 && tp != 0) {
            Serial.println("go 0;");
            // per tornare alla posizione 0 serve move(-200)
            tp = 0;
            g_state = 0;
        }
        break;
    
  

    }
      
    
  
#endif         
     
  
          
         
     
      
      
  


  

  
  

  
}