#include "Atm_AccelStepper.h"

/* Add optional parameters for the state machine to begin()
 * Add extra initialization code
 */

Atm_AccelStepper& Atm_AccelStepper::begin(int step_pin, int dir_pin) {
  // clang-format off
  const static state_t state_table[] PROGMEM = {
    /*                       ON_ENTER           ON_LOOP          ON_EXIT  EVT_DISABLE  EVT_ENABLE  EVT_ENABLED_TIMEOUT    EVT_MOVE  EVT_STOP  EVT_EMERGENCY_STOP  EVT_ON_LIMIT_LOW  EVT_ON_LIMIT_HIGH  EVT_ON_TARGET  EVT_HOMING_LOW  EVT_HOMING_HIGH  ELSE */
    /*    DISABLE */     ENT_DISABLED,              -1,              -1,         -1,    ENABLED,                  -1,     RUNNING,        -1,                -1,               -1,                -1,            -1,    HOMING_LOW,     HOMING_HIGH,   -1,
    /*     ENABLED */     ENT_ENABLED,              -1,              -1,    DISABLE,         -1,             DISABLE,     RUNNING,      STOP,              STOP,        LIMIT_LOW,        LIMIT_HIGH,            -1,    HOMING_LOW,     HOMING_HIGH,   -1,
    /*     RUNNING */     ENT_RUNNING,      LP_RUNNING,              -1,    DISABLE,         -1,                  -1,     RUNNING,      STOP,              STOP,        LIMIT_LOW,        LIMIT_HIGH,       ENABLED,            -1,              -1,   -1,
    /*        STOP */        ENT_STOP,         LP_STOP,              -1,    DISABLE,         -1,                  -1,     RUNNING,        -1,                -1,               -1,                -1,       ENABLED,            -1,              -1,   -1,
    /*  HOMING_LOW */  ENT_HOMING_LOW,   LP_HOMING_LOW,  EXT_HOMING_LOW,    DISABLE,         -1,                  -1,          -1,      STOP,              STOP,        LIMIT_LOW,        LIMIT_HIGH,            -1,            -1,              -1,   -1,
    /* HOMING_HIGH */ ENT_HOMING_HIGH,  LP_HOMING_HIGH, EXT_HOMING_HIGH,    DISABLE,         -1,                  -1,          -1,      STOP,              STOP,        LIMIT_LOW,        LIMIT_HIGH,            -1,            -1,              -1,   -1,
    /*   LIMIT_LOW */   ENT_LIMIT_LOW,    LP_LIMIT_LOW,              -1,         -1,         -1,                  -1,     RUNNING,      STOP,              STOP,        LIMIT_LOW,                -1,            -1,            -1,              -1,   -1,
    /*  LIMIT_HIGH */  ENT_LIMIT_HIGH,   LP_LIMIT_HIGH,              -1,         -1,         -1,                  -1,     RUNNING,      STOP,              STOP,               -1,        LIMIT_HIGH,            -1,            -1,              -1,   -1
  };
  // clang-format on
  Machine::begin( state_table, ELSE );
  stepper = new AccelStepper(1, step_pin, dir_pin);
  stepper->setMaxSpeed(max_speed);
  stepper->setAcceleration(acceleration);
  idle_timer.set(ATM_TIMER_OFF);
  position_timer.set(POSITION_SEND_TIMER);

  return *this;
}

/* Add C++ code for each internally handled event (input)
 * The code must return 1 to trigger the event
 */

int Atm_AccelStepper::event( int id ) {
  //updateLimitSwitch();
  switch ( id ) {
    case EVT_DISABLE:
      return 0;
    case EVT_ENABLE:
      return 0;
    case EVT_ENABLED_TIMEOUT:
      return 0;
    case EVT_MOVE:
      return 0;
    case EVT_STOP:
      return 0;
    case EVT_EMERGENCY_STOP:
      return 0;
    case EVT_ON_LIMIT_LOW:
      previous_state = limitLow_State;
      switch(_limitLow_Mode) {
        case 0:
          return 0;
        case 1: //digital INPUT
          // Serial.println("digital");
          limitLow_State = digitalRead(_limitLow_Pin);
          limitLow_State = _limitLow_Reversed ? !limitLow_State : limitLow_State;
          return limitLow_State != previous_state;
        case 2:
          int analogTemp = analogRead(_limitLow_Pin);
          limitLow_State = (_limitLow_Thresholds[0] < analogTemp) && (analogTemp < _limitLow_Thresholds[1]);
          limitLow_State = _limitLow_Reversed ? !limitLow_State : limitLow_State;
          if(limitLow_State){analogTemp = analogRead(_limitLow_Pin);
          limitLow_State = (_limitLow_Thresholds[0] < analogTemp) && (analogTemp < _limitLow_Thresholds[1]);
          limitLow_State = _limitLow_Reversed ? !limitLow_State : limitLow_State;}
          return limitLow_State != previous_state;
      }
    case EVT_ON_LIMIT_HIGH:
      previous_state = limitHigh_State;
      switch(_limitHigh_Mode) {
        case 0:
          return 0;
        case 1: //digital INPUT
          limitHigh_State = digitalRead(_limitHigh_Pin);
          limitHigh_State = _limitHigh_Reversed ? !limitHigh_State : limitHigh_State;
          return limitHigh_State != previous_state;
        case 2:
        //Serial.println("analog");
          int analogTemp = analogRead(_limitHigh_Pin);
          limitHigh_State = (_limitHigh_Thresholds[0] < analogTemp) && (analogTemp < _limitHigh_Thresholds[1]);
          limitHigh_State = _limitHigh_Reversed ? !limitHigh_State : limitHigh_State;
          return limitHigh_State != previous_state;
      }
    case EVT_ON_TARGET:
      return runMode ? 0 : _currentStep == _targetStep;
    case EVT_HOMING_LOW:
      return 0;
    case EVT_HOMING_HIGH:
      return 0;
  }
  return 0;
}

/* Add C++ code for each action
 * This generates the 'output' for the state machine
 *
 * Available connectors:
 *   push( connectors, ON_CHANGEPOSITION, 0, <v>, <up> );
 *   push( connectors, ON_CHANGESTATE, 0, <v>, <up> );
 *   push( connectors, ON_ONLIMITHIGH, 0, <v>, <up> );
 *   push( connectors, ON_ONLIMITLOW, 0, <v>, <up> );
 *   push( connectors, ON_ONTARGET, 0, <v>, <up> );
 *   push( connectors, ON_STOP, 0, <v>, <up> );
 */

void Atm_AccelStepper::action( int id ) {
  switch ( id ) {
    case ENT_DISABLED:
      push(connectors, ON_CHANGESTATE, 0,  state(), 0);
      enabled = _enableReversed ? HIGH : LOW;
      digitalWrite(_enablePin, enabled);
      return;
    case ENT_ENABLED:
      _isHoming = 0 ;
      if(last_trigger == EVT_ON_TARGET){push( connectors, ON_ONTARGET, 0, _currentStep, 0 );};
      push(connectors, ON_CHANGESTATE, 0,  state(), 0);
      push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
      enabled = _enableReversed ? LOW : HIGH ;
      digitalWrite(_enablePin, enabled);


      return;
    case ENT_RUNNING:
      push(connectors, ON_CHANGESTATE, 0,  state(), 0);
      push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
      _isHoming = 0;
      //reset limit state so that they trigger again if we're stopped on it
      // limitLow_State = 0;
      // limitHigh_State = 0;
      Serial.print("target ");
      Serial.println(_targetStep);
      stepper->moveTo(_targetStep);
      // stepper->computeNewSpeed();
      //stepper_update();
      //push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
      position_timer.setFromNow(this, POSITION_SEND_TIMER);
      return;
    case LP_RUNNING:
      stepper_update();
      if(stepper->speed() == 0.) {trigger(EVT_ON_TARGET);}
      return;
    case ENT_STOP:
    push(connectors, ON_CHANGESTATE, 0,  state(), 0);
    push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
      if (last_trigger == EVT_STOP) {
        // runMode = 0 ;
        stepper->stop();
        _targetStep = stepper->targetPosition();
        push( connectors, ON_STOP, 0, 0, 0 );
      }
      if (last_trigger == EVT_EMERGENCY_STOP) {
        stepper->setSpeed(0);
        _currentStep = stepper->currentPosition();
        _targetStep = _currentStep ;
        stepper->moveTo(_targetStep);
        push( connectors, ON_STOP, 0, 1, 0 );
      }
      return;
    case LP_STOP:
      stepper_update();
      push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
      if(stepper->speed() == 0.) {trigger(EVT_ON_TARGET);}
      // _currentStep = stepper->currentPosition();
      return;
    case ENT_HOMING_LOW:
      push(connectors, ON_CHANGESTATE, 0,  state(), 0);
      push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
      runMode = 1;
      _isHoming = 1 ;
      stepper->setSpeed(-1*homing_speed);
      return;
    case LP_HOMING_LOW:
      stepper_update();
      push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
      return;
    case EXT_HOMING_LOW:
      runMode = 0;
      _isHoming = 0;
      push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
      if(last_trigger == EVT_ON_LIMIT_LOW) {
        stepper->setCurrentPosition(0);
        _currentStep = 0;
        push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
        Serial.println("homing low done");
      }
      else{Serial.println("homing low failed");}
      trigger(EVT_EMERGENCY_STOP);

      return;
    case ENT_HOMING_HIGH:
      push(connectors, ON_CHANGESTATE, 0,  state(), 0);
      push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
      runMode = 1;
      _isHoming = 2 ;
      stepper->setSpeed(homing_speed);
      return;
    case LP_HOMING_HIGH:
      stepper_update();
      push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
      return;
    case EXT_HOMING_HIGH:
    push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
      runMode = 0;
      _isHoming = 0;
      trigger(EVT_EMERGENCY_STOP);
      if(last_trigger == EVT_ON_LIMIT_HIGH) {
        _maxStep = stepper->currentPosition();
        _currentStep = _maxStep;
        Serial.println("homing high done");
      }
      else{Serial.println("homing high failed");}
      _targetStep = _currentStep;
      return;
    case ENT_LIMIT_LOW:
      /*triggered by a change in limit state
       if state is 0, we may leave this state for running
       if state is 1 we stay in limit state loop, where moves are allowed only in
            the free direction, until a trigger comes with state 0
      */
      push( connectors, ON_ONLIMITLOW, 0, limitLow_State, 0 );
      push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
      if (!limitLow_State){trigger(EVT_MOVE);}
      return;

    case LP_LIMIT_LOW:
      //stop motor if going down, allow going up

      if(_limitLow_Hard && (_targetStep < _currentStep)) {
          // Serial.println("youpi");
          _currentStep = stepper->currentPosition();
          stepper->moveTo(_currentStep);
          _targetStep = _currentStep;
          stepper->setSpeed(0);
        }
        stepper_update();
      //else{} // _isHoming ? trigger(EVT_STOP):
      return;
    case ENT_LIMIT_HIGH:
      push( connectors, ON_ONLIMITHIGH, 0, limitHigh_State, 0 );
      push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
      if (!limitHigh_State){trigger(EVT_MOVE);};
      return;
    case LP_LIMIT_HIGH:
      //stop motor if going down, allow going up

      if(_limitHigh_Hard && (_targetStep > _currentStep)) {
          // Serial.println("youpi");
          _currentStep = stepper->currentPosition();
          stepper->moveTo(_currentStep);
          _targetStep = _currentStep;
          stepper->setSpeed(0);
        }
        stepper_update();
      // else{}
      return;
  }
}

/* Optionally override the default trigger() method
 * Control how your machine processes triggers
 */

Atm_AccelStepper& Atm_AccelStepper::trigger( int event ) {
  Machine::trigger( event );
  return *this;
}

/* Optionally override the default state() method
 * Control what the machine returns when another process requests its state
 */

int Atm_AccelStepper::state( void ) {
  return Machine::state();
}

/* Nothing customizable below this line
 ************************************************************************************************
*/

/* Still I'll customize a little just here
*/

void Atm_AccelStepper::stepper_update(void) {


  switch (runMode) {
    case 0: //positional modae
      stepper->run();
      break;
    case 1: // speed mode
      stepper->runSpeed();
      break;
  }
  // Serial.print("update ");
  // Serial.println(stepper->speed());
  long int tempStep = stepper->currentPosition();
  if (tempStep != _currentStep){
    _currentStep =  tempStep;
    //Serial.println(stepper->currentPosition());
    if (position_timer.expired(this)){
      push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, stepper->speed());
      position_timer.setFromNow(this, POSITION_SEND_TIMER);
    }
  }
}

Atm_AccelStepper& Atm_AccelStepper::setMaxSpeed( long int maxSpeed){
  max_speed = maxSpeed ;
  stepper->setMaxSpeed(max_speed);
  return *this ;
}

Atm_AccelStepper& Atm_AccelStepper::setHomingSpeed(long int homingSpeed){
  homing_speed = homingSpeed ;
  return *this ;
}

Atm_AccelStepper& Atm_AccelStepper::setAcceleration(long int acc){
  acceleration = acc ;
  stepper->setAcceleration(acceleration);
  return *this ;
}

Atm_AccelStepper& Atm_AccelStepper::setPosition(long int position){
  stepper->setCurrentPosition(position);
  _currentStep = position ;
  return *this ;
}

long int Atm_AccelStepper::getPosition(){
  return stepper->currentPosition();;
}

long int Atm_AccelStepper::distanceToGo(){
  return stepper->distanceToGo();;
}


bool Atm_AccelStepper::isRunning(){
  return stepper->isRunning();
}

float Atm_AccelStepper::getSpeed(){
  return stepper->speed();
}

Atm_AccelStepper& Atm_AccelStepper::position_refresh(long int refresh_ms){
  POSITION_SEND_TIMER = refresh_ms ;
  return *this ;
}

Atm_AccelStepper& Atm_AccelStepper::move( long int stepRel) {
  _targetStep   = _currentStep + stepRel;
  runMode = 0;
  _isHoming = 0;
  //Serial.println(_targetStep);
  stepper->moveTo(_targetStep);
  enable();
  trigger( EVT_MOVE );
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::moveTo( long int stepAbs) {
  _targetStep   = stepAbs;
  _isHoming = 0 ;
  runMode = 0;
  stepper->moveTo(_targetStep);
  enable();
  trigger( EVT_MOVE );
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::rotate( long  int speed) {
  runMode = 1;
  _isHoming = 0 ;
  stepper->setSpeed( speed);
  enable();
  trigger( EVT_MOVE );
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::homing( bool direction ){
  enable();
  direction == 1 ? _isHoming = 2 : _isHoming = 1;
  direction == 1 ? this->trigger(EVT_HOMING_HIGH) : this->trigger(EVT_HOMING_LOW);

  return *this;
}

// Atm_AccelStepper& Atm_AccelStepper::rotationReversed(bool reversed){
//   _rotationReversed = reversed ? -1 : 1 ;
// }

Atm_AccelStepper& Atm_AccelStepper::setEnablePin( int enablePin ){
  _enablePin = enablePin ;
  pinMode(_enablePin, OUTPUT);
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::pinReversed( bool directionInvert,
                              bool stepInvert, bool enableInvert){
  stepper->setPinsInverted(directionInvert, stepInvert, enableInvert);
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::limitLow_set(int mode,  int pin,  int reversed){
  _limitLow_Mode = mode ;
  _limitLow_Pin = pin ;
  _limitLow_Reversed = reversed ;
  if (_limitLow_Mode==1) {pinMode(_limitLow_Pin, INPUT_PULLUP);}
  if (_limitLow_Mode==2) {pinMode(_limitLow_Pin, INPUT);}
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::limitLow_isHard(bool hardlimit){
  _limitLow_Hard = hardlimit;
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::limitLow_setThresholds (int threshold_low, int threshold_high){
  _limitLow_Thresholds[0] = threshold_low ;
  _limitLow_Thresholds[1] = threshold_high ;
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::limitHigh_set(int mode,  int pin,  int reversed){
  _limitHigh_Mode = mode ;
  _limitHigh_Pin = pin ;
  _limitHigh_Reversed = reversed ;
  if (_limitHigh_Mode==1) {pinMode(_limitHigh_Pin, INPUT_PULLUP);}
  if (_limitHigh_Mode==2) {pinMode(_limitHigh_Pin, INPUT);}
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::limitHigh_isHard(bool hardlimit){
  _limitHigh_Hard = hardlimit;
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::limitHigh_setThresholds (int threshold_low, int threshold_high){
  _limitHigh_Thresholds[0] = threshold_low ;
  _limitHigh_Thresholds[1] = threshold_high ;
  return *this;
}

/* Public event methods
 *
 */

Atm_AccelStepper& Atm_AccelStepper::disable() {
  trigger( EVT_DISABLE );
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::enable() {
  trigger( EVT_ENABLE );
  return *this;
}

// Atm_AccelStepper& Atm_AccelStepper::move() {
//   trigger( EVT_MOVE );
//   return *this;
// }

Atm_AccelStepper& Atm_AccelStepper::stop() {
  trigger( EVT_STOP );
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::emergency_stop() {
  trigger( EVT_EMERGENCY_STOP );
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::on_limit_low() {
  trigger( EVT_ON_LIMIT_LOW );
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::on_limit_high() {
  trigger( EVT_ON_LIMIT_HIGH );
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::on_target() {
  trigger( EVT_ON_TARGET );
  return *this;
}



/*
 * onChangeposition() push connector variants ( slots 1, autostore 0, broadcast 0 )
 */

Atm_AccelStepper& Atm_AccelStepper::onChangeposition( Machine& machine, int event ) {
  onPush( connectors, ON_CHANGEPOSITION, 0, 1, 1, machine, event );
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::onChangeposition( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_CHANGEPOSITION, 0, 1, 1, callback, idx );
  return *this;
}

/*
 * onChangestate() push connector variants ( slots 1, autostore 0, broadcast 0 )
 */

Atm_AccelStepper& Atm_AccelStepper::onChangestate( Machine& machine, int event ) {
  onPush( connectors, ON_CHANGESTATE, 0, 1, 1, machine, event );
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::onChangestate( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_CHANGESTATE, 0, 1, 1, callback, idx );
  return *this;
}

/*
 * onOnlimithigh() push connector variants ( slots 1, autostore 0, broadcast 0 )
 */

Atm_AccelStepper& Atm_AccelStepper::onOnlimithigh( Machine& machine, int event ) {
  onPush( connectors, ON_ONLIMITHIGH, 0, 1, 1, machine, event );
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::onOnlimithigh( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_ONLIMITHIGH, 0, 1, 1, callback, idx );
  return *this;
}

/*
 * onOnlimitlow() push connector variants ( slots 1, autostore 0, broadcast 0 )
 */

Atm_AccelStepper& Atm_AccelStepper::onOnlimitlow( Machine& machine, int event ) {
  onPush( connectors, ON_ONLIMITLOW, 0, 1, 1, machine, event );
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::onOnlimitlow( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_ONLIMITLOW, 0, 1, 1, callback, idx );
  return *this;
}

/*
 * onOntarget() push connector variants ( slots 1, autostore 0, broadcast 0 )
 */

Atm_AccelStepper& Atm_AccelStepper::onOntarget( Machine& machine, int event ) {
  onPush( connectors, ON_ONTARGET, 0, 1, 1, machine, event );
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::onOntarget( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_ONTARGET, 0, 1, 1, callback, idx );
  return *this;
}

/*
 * onStop() push connector variants ( slots 1, autostore 0, broadcast 0 )
 */

Atm_AccelStepper& Atm_AccelStepper::onStop( Machine& machine, int event ) {
  onPush( connectors, ON_STOP, 0, 1, 1, machine, event );
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::onStop( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_STOP, 0, 1, 1, callback, idx );
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::onOnhominglow( Machine& machine, int event ) {
  onPush( connectors, ON_ONTARGET, 0, 1, 1, machine, event );
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::onOnhominglow( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_ONTARGET, 0, 1, 1, callback, idx );
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::onOnhominghigh( Machine& machine, int event ) {
  onPush( connectors, ON_ONTARGET, 0, 1, 1, machine, event );
  return *this;
}

Atm_AccelStepper& Atm_AccelStepper::onOnhominghigh( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_ONTARGET, 0, 1, 1, callback, idx );
  return *this;
}

/* State trace method
 * Sets the symbol table and the default logging method for serial monitoring
 */

Atm_AccelStepper& Atm_AccelStepper::trace( Stream & stream ) {
  Machine::setTrace( &stream, atm_serial_debug::trace,
    "ACCELSTEPPER\0EVT_DISABLE\0EVT_ENABLE\0EVT_ENABLED_TIMEOUT\0EVT_MOVE\0EVT_STOP\0EVT_EMERGENCY_STOP\0EVT_ON_LIMIT_LOW\0EVT_ON_LIMIT_HIGH\0EVT_ON_TARGET\0EVT_HOMING_LOW\0EVT_HOMING_HIGH\0ELSE\0DISABLED\0ENABLED\0RUNNING\0STOP\0HOMING_LOW\0HOMING_HIGH\0LIMIT_LOW\0LIMIT_HIGH" );
  return *this;
}