#if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__)
#ifdef TEENSYSTEP_H
#include "Atm_Teenstep.h"

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

Atm_Teenstep& Atm_Teenstep::begin(Stepper & motorRef, StepControl & stepControlRef) {
  // clang-format off
  const static state_t state_table[] PROGMEM = {
    /*                             ON_ENTER     ON_LOOP          ON_EXIT  EVT_HOMING_LOW  EVT_HOMING_HIGH  EVT_MOVE_TIMEOUT  EVT_LIMIT_HIGH   EVT_LIMIT_LOW  EVT_EMERGENCYSTOP  EVT_STOP  EVT_ONTARGET  EVT_MOVE  EVT_DISABLE  EVT_ENABLE  ELSE */
    /*       DISABLED */       ENT_DISABLED,         -1,              -1,     HOMING_LOW,     HOMING_HIGH,               -1,             -1,             -1,                -1,       -1,           -1,       -1,          -1,    ENABLED,   -1,
    /*        ENABLED */        ENT_ENABLED,         -1,              -1,     HOMING_LOW,     HOMING_HIGH,               -1,             -1,             -1,                -1,       -1,           -1,  RUNNING,    DISABLED,         -1,   -1,
    /*        RUNNING */        ENT_RUNNING, LP_RUNNING,              -1,     HOMING_LOW,     HOMING_HIGH,               -1,             -1,             -1,    EMERGENCY_STOP, STOPPING,      ENABLED,  RUNNING,          -1,         -1,   -1,
    /*       STOPPING */       ENT_STOPPING,         -1,              -1,             -1,              -1,               -1, EMERGENCY_STOP, EMERGENCY_STOP,    EMERGENCY_STOP,       -1,           -1,  RUNNING,          -1,         -1,   -1,
    /* EMERGENCY_STOP */ ENT_EMERGENCY_STOP,         -1,              -1,             -1,              -1,               -1,             -1,             -1,                -1,       -1,           -1,  RUNNING,          -1,    ENABLED,   -1,
    /*    HOMING_HIGH */    ENT_HOMING_HIGH,         -1, EXT_HOMING_HIGH,             -1,              -1,               -1,        ENABLED,             -1,    EMERGENCY_STOP, STOPPING,           -1,       -1,          -1,         -1,   -1,
    /*     HOMING_LOW */     ENT_HOMING_LOW,         -1,  EXT_HOMING_LOW,             -1,              -1,               -1,            -1,        ENABLED,    EMERGENCY_STOP, STOPPING,           -1,       -1,          -1,         -1,   -1,
  };
  // clang-format on
  Machine::begin( state_table, ELSE );
  this-> motor =   &motorRef;
  this-> controller = &stepControlRef;
  return *this;
}

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

int Atm_Teenstep::event( int id ) {
  updateLimitSwitch();
  switch ( id ) {
    case EVT_HOMING_LOW:
      return 0;
    case EVT_HOMING_HIGH:
      return 0;
    case EVT_MOVE_TIMEOUT:
      return 0;
    case EVT_LIMIT_HIGH:
      return limitState[0];
    case EVT_LIMIT_LOW:
      return limitState[1];
    case EVT_EMERGENCYSTOP:
      return 0;
    case EVT_STOP:
      return 0;
    case EVT_ONTARGET:
      return _currentStep == _targetStep;
    case EVT_MOVE:
      return 0;
    case EVT_DISABLE:
      return 0;
    case EVT_ENABLE:
      return 0;
  }
  return 0;
}

/* Add C++ code for each action
 * This generates the 'output' for the state machine
 *
 * Available connectors:
 *   push( connectors, ON_CHANGE, 0, <v>, <up> );
 *   push( connectors, ON_CHANGEPOSITION, 0, <v>, <up> );
 *   push( connectors, ON_LIMITHIGH, 0, <v>, <up> );
 *   push( connectors, ON_LIMITLOW, 0, <v>, <up> );
 */

void Atm_Teenstep::action( int id ) {
  switch ( id ) {
    long int tempStep ;
    case ENT_DISABLED:
      push(connectors, ON_CHANGE, 0,  state(), 0);
      enabled = _enableReversed ? HIGH : LOW;
      digitalWrite(_enablePin, enabled);
      sendOSC();
      return;
    case ENT_ENABLED:
      push(connectors, ON_CHANGE, 0,  state(), 0);
      enabled = _enableReversed ? LOW : HIGH ;
      digitalWrite(_enablePin, enabled);
      sendOSC();
      return;
    case ENT_RUNNING:
      push(connectors, ON_CHANGE, 0,  state(), 0);
      push(connectors, ON_CHANGE, 0,  2, 0);
      return;
    case LP_RUNNING:
      tempStep = motor->getPosition();
      if (tempStep != _currentStep){
        _currentStep =  tempStep;
        push(connectors, ON_CHANGEPOSITION, 0,  _currentStep, 0);
      }

      //updateLimitSwitch();
      sendOSC();
      return;
    case ENT_STOPPING:
      push(connectors, ON_CHANGE, 0,  state(), 0);
      sendOSC();
      return;
    case ENT_EMERGENCY_STOP:
      push(connectors, ON_CHANGE, 0,  state(), 0);
      controller->emergencyStop();
      trigger(EVT_ENABLE);
      sendOSC();
      return;
    case ENT_HOMING_HIGH:
      push(connectors, ON_CHANGE, 0,  state(), 0);
      if(_limitType) {
        motor->setTargetRel(2147483647);
        controller->moveAsync(*motor);
        }
      sendOSC();
      return;
    case EXT_HOMING_HIGH:
      controller->emergencyStop();
      if(last_trigger == EVT_LIMIT_HIGH){
        _maxStep = motor->getPosition();
        push(connectors, ON_LIMITHIGH, 0,  _maxStep, 0);
        Serial.print("Stepper maxPos ");
        Serial.println(motor->getPosition());
        trigger(EVT_ENABLE);
      }
      else{
        push(connectors, ON_LIMITLOW, 0,  1, 0);
        Serial.println("homing failed ");
        trigger(EVT_EMERGENCYSTOP);
      }

      return;
    case ENT_HOMING_LOW:
      push(connectors, ON_CHANGE, 0,  state(), 0);
      if(_limitType) {
        motor->setTargetRel(-2147483647);
        controller->moveAsync(*motor);
        }
      sendOSC();
      return;
    case EXT_HOMING_LOW:
      controller->emergencyStop();
      if(last_trigger == EVT_LIMIT_LOW){
        push(connectors, ON_LIMITLOW, 0,  1, 0);
        motor->setPosition(0);
        Serial.print("Stepper homed ");
        Serial.println(motor->getPosition());
        trigger(EVT_ENABLE);
      }
      else{
        push(connectors, ON_LIMITHIGH, 0,  1, 0);
        Serial.println("homing failed ");
        trigger(EVT_EMERGENCYSTOP);
      }


      return;
  }
}

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

Atm_Teenstep& Atm_Teenstep::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_Teenstep::state( void ) {
  return Machine::state();
}

/* CUSTOM METHODS
********************************************************************************************************
*/

// Atm_TeensyStep& Atm_TeensyStep::enable( bool enable ){
//
//   return *this;
// }

/////////   ENABLE/DISABLE    ////////

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

Atm_Teenstep& Atm_Teenstep::enableReversed( bool reverse ){
  _enableReversed = reverse ;
  return *this;
}

/////////   LIMITS    ////////

Atm_Teenstep& Atm_Teenstep::setLimitPins( int limitPinLow){
  _limitPin[0] = limitPinLow;
  pinMode(_limitPin[0], INPUT);
  return *this;
}

Atm_Teenstep& Atm_Teenstep::setLimitPins( int limitPinLow, int limitPinHigh){
 _limitPin[0] = limitPinLow;
 _limitPin[1] = limitPinHigh;
 pinMode(_limitPin[0], INPUT);
 pinMode(_limitPin[1], INPUT);
 return *this;
}

Atm_Teenstep& Atm_Teenstep::setLimitType( int limitType){
 _limitType = limitType;
 return *this;
}

Atm_Teenstep& Atm_Teenstep::limitReversed( bool reversed){
 _limitReversed = reversed;
 return *this;
}

Atm_Teenstep& Atm_Teenstep::limitThresholds( int limitThreshold0,
       int limitThreshold1,  int limitThreshold2, int limitThreshold3){
 _limitThresholds[0] = limitThreshold0;
 _limitThresholds[1] = limitThreshold1;
 _limitThresholds[2] = limitThreshold2;
 _limitThresholds[3] = limitThreshold3;
 return *this;
}

void Atm_Teenstep::updateLimitSwitch(){
  switch (_limitType) { // limitType!=0 means there is limit to check
    case NONE:
      return ;
    case DIGITAL_1:
      limitState[0] = digitalRead(_limitPin[0]);
      limitState[0] = _limitReversed ? !limitState[0] : limitState[0];
      return;
    case DIGITAL_2:
    limitState[0] = digitalRead(_limitPin[0]);
    limitState[1] = digitalRead(_limitPin[1]);
    limitState[0] = _limitReversed ? !limitState[0] : limitState[0];
    limitState[1] = _limitReversed ? !limitState[1] : limitState[1];
      return;
    case ANALOG_1:
    int read = analogRead(_limitPin[0]) ;
    limitState[0] = _limitThresholds[0] < read && read < _limitThresholds[1] ;
    limitState[1] = _limitThresholds[2] < read && read < _limitThresholds[3] ;
      return;
  }

}


Atm_Teenstep& Atm_Teenstep::onOSC( void ){
//  _enableReversed = reverse ;
  return *this;
}

Atm_Teenstep& Atm_Teenstep::sendOSC( void ){
//  _enableReversed = reverse ;
  return *this;
}

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

/* Public event methods
 *
 */
 Atm_Teenstep& Atm_Teenstep::homing_low() {
   trigger( EVT_HOMING_LOW );
   return *this;
 }

 Atm_Teenstep& Atm_Teenstep::homing_high() {
   trigger( EVT_HOMING_HIGH );
   return *this;
 }

Atm_Teenstep& Atm_Teenstep::move_timeout() {
  trigger( EVT_MOVE_TIMEOUT );
  return *this;
}

Atm_Teenstep& Atm_Teenstep::limit_high() {
  trigger( EVT_LIMIT_HIGH );
  return *this;
}

Atm_Teenstep& Atm_Teenstep::limit_low() {
  trigger( EVT_LIMIT_LOW );
  return *this;
}

Atm_Teenstep& Atm_Teenstep::emergencystop() {
  trigger( EVT_EMERGENCYSTOP );
  return *this;
}

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

Atm_Teenstep& Atm_Teenstep::ontarget() {
  trigger( EVT_ONTARGET );
  return *this;
}

Atm_Teenstep& Atm_Teenstep::move(long int stepRel) {
  _targetStep   = _currentStep + stepRel;
  //Serial.println(_targetStep);
  motor->setTargetAbs(_targetStep);
  controller->moveAsync(*motor);
  enable();
  trigger( EVT_MOVE );
  return *this;
}

Atm_Teenstep& Atm_Teenstep::moveTo(long int stepAbs) {
  _targetStep   = stepAbs;
  motor->setTargetAbs(_targetStep);
  controller->moveAsync(*motor);
  enable();
  trigger( EVT_MOVE );
  return *this;
}

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

Atm_Teenstep& Atm_Teenstep::enable() {
  trigger( EVT_ENABLE );
}

Atm_Teenstep& Atm_Teenstep::homing(bool direction) {
  direction ? trigger( EVT_HOMING_HIGH ) : trigger(EVT_HOMING_LOW);
  return *this;
}

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

Atm_Teenstep& Atm_Teenstep::onChange( Machine& machine, int event ) {
  onPush( connectors, ON_CHANGE, 0, 1, 1, machine, event );
  return *this;
}

Atm_Teenstep& Atm_Teenstep::onChange( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_CHANGE, 0, 1, 1, callback, idx );
  return *this;
}

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

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

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

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

Atm_Teenstep& Atm_Teenstep::onLimithigh( Machine& machine, int event ) {
  onPush( connectors, ON_LIMITHIGH, 0, 1, 1, machine, event );
  return *this;
}

Atm_Teenstep& Atm_Teenstep::onLimithigh( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_LIMITHIGH, 0, 1, 1, callback, idx );
  return *this;
}

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

Atm_Teenstep& Atm_Teenstep::onLimitlow( Machine& machine, int event ) {
  onPush( connectors, ON_LIMITLOW, 0, 1, 1, machine, event );
  return *this;
}

Atm_Teenstep& Atm_Teenstep::onLimitlow( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_LIMITLOW, 0, 1, 1, callback, idx );
  return *this;
}

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

 Atm_Teenstep& Atm_Teenstep::trace( Stream & stream ) {
   Machine::setTrace( &stream, atm_serial_debug::trace,
     "TEENSTEP\0EVT_HOMING_LOW\0EVT_HOMING_HIGH\0EVT_MOVE_TIMEOUT\0EVT_LIMIT_HIGH\0EVT_LIMIT_LOW\0EVT_EMERGENCYSTOP\0EVT_STOP\0EVT_ONTARGET\0EVT_MOVE\0EVT_DISABLE\0EVT_ENABLE\0ELSE\0DISABLED\0ENABLED\0RUNNING\0STOPPING\0EMERGENCY_STOP\0HOMING_HIGH\0HOMING_LOW" );
   return *this;
 }

#endif
#endif