#include "Atm_Tstepper.h"

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

Atm_Tstepper& Atm_Tstepper::begin(Stepper & motorRef, StepControl & stepControlRef) {
  // clang-format off
  const static state_t state_table[] PROGMEM = {
    /*                              ON_ENTER      ON_LOOP     ON_EXIT  EVT_HOMING_COUNTER  EVT_IDLE_TIMEOUT  EVT_MOVE_TIMEOUT  EVT_LIMIT_LOW  EVT_LIMIT_HIGH  EVT_EMERGENCY  EVT_STOP  EVT_ONTARGET  EVT_MOVE  EVT_DISABLE  EVT_ENABLE  EVT_HOMING  ELSE */
    /*          DISABLED */     ENT_DISABLED,          -1,         -1,                 -1,               -1,               -1,            -1,             -1,            -1,       -1,           -1,       -1,          -1,    ENABLED,        -1,    -1,
    /*           ENABLED */      ENT_ENABLED,          -1,         -1,                 -1,         DISABLED,               -1,            -1,             -1,            -1,       -1,           -1,  RUNNING,    DISABLED,         -1,    HOMING,    -1,
    /*           RUNNING */      ENT_RUNNING,  LP_RUNNING,         -1,                 -1,               -1,     MOVE_TIMEOUT,     EMERGENCY,      EMERGENCY,     EMERGENCY, STOPPING,      ENABLED,  RUNNING,          -1,         -1,        -1,    -1,
    /*          STOPPING */     ENT_STOPPING, LP_STOPPING,         -1,                 -1,               -1,               -1,     EMERGENCY,      EMERGENCY,     EMERGENCY,       -1,           -1,  RUNNING,          -1,         -1,        -1,    -1,
    /*         EMERGENCY */    ENT_EMERGENCY,          -1,         -1,                 -1,               -1,               -1,            -1,             -1,            -1,       -1,           -1,       -1,          -1,         -1,        -1,    -1,
    /*            HOMING */       ENT_HOMING,   LP_HOMING, EXT_HOMING,                 -1,               -1,     MOVE_TIMEOUT,     EMERGENCY,      EMERGENCY,     EMERGENCY, STOPPING, MOVE_TIMEOUT,       -1,          -1,         -1,        -1,    -1,
    /*      MOVE_TIMEOUT */ ENT_MOVE_TIMEOUT,          -1,         -1,                 -1,               -1,               -1,     EMERGENCY,             -1,     EMERGENCY,       -1,           -1,       -1,          -1,         -1,        -1,    -1,
    /* HOMING_COUNTEROUT */               -1,          -1,         -1,                 -1,               -1,               -1,            -1,             -1,            -1,       -1,           -1,       -1,          -1,         -1,        -1,    -1
  };
  // clang-format on
  Machine::begin( state_table, ELSE );
  this-> motor =   &motorRef;
  this-> controller = &stepControlRef;
  idle_timer.set(ATM_TIMER_OFF);
  moving_timer.set(ATM_TIMER_OFF);
  return *this;
}

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

int Atm_Tstepper::event( int id ) {
  switch ( id ) {
    case EVT_HOMING_COUNTER:
      return 0;
    case EVT_IDLE_TIMEOUT:
      return 0; //idle_timer.expired(this);
    case EVT_MOVE_TIMEOUT:
      return 0; //moving_timer.expired(this);
    case EVT_LIMIT_LOW:
      return limitState[0];//limitState[0];
    case EVT_LIMIT_HIGH:
      return limitState[1];///limitState[1];
    case EVT_EMERGENCY:
      return 0;
    case EVT_STOP:
      return 0;
    case EVT_ONTARGET: //triggers if stepper reached _targetStep
      return _currentStep == _targetStep ;
    case EVT_MOVE:
      return 0;
    case EVT_DISABLE:
      return 0;
    case EVT_ENABLE:
      return 0;
    case EVT_HOMING:
      return 0;
  }
  return 0;
}

/* Add C++ code for each action
 * This generates the 'output' for the state machine
 *
 * Available connectors:
 *   push( connectors, ON_ONCHANGE, 0, <v>, <up> );
 *   push( connectors, ON_ONCHANGEACCELERATION, 0, <v>, <up> );
 *   push( connectors, ON_ONCHANGEPOSITION, 0, <v>, <up> );
 *   push( connectors, ON_ONCHANGESTATE, 0, <v>, <up> );
 *   push( connectors, ON_ONHIGHLIMIT, 0, <v>, <up> );
 *   push( connectors, ON_ONHOMINGCOUNTEROUT, 0, <v>, <up> );
 *   push( connectors, ON_ONHOMINGTIMEOUT, 0, <v>, <up> );
 *   push( connectors, ON_ONIDLETIMEOUT, 0, <v>, <up> );
 *   push( connectors, ON_ONLOWLIMIT, 0, <v>, <up> );
 *   push( connectors, ON_ONMOVETIMEOUT, 0, <v>, <up> );
 *   push( connectors, ON_SPEED, 0, <v>, <up> );
 */

void Atm_Tstepper::action( int id ) {
  switch ( id ) {
    long int tempStep ;
    case ENT_DISABLED:
      idle_timer.set(ATM_TIMER_OFF);
      moving_timer.set(ATM_TIMER_OFF);
      enabled = _enableReversed ? HIGH : LOW;
      digitalWrite(_enablePin, enabled);
      sendOSC();
      return;
    case ENT_ENABLED:
      // idle_timer.set(IDLE_TIMEOUT_DURATION);
      // moving_timer.set(ATM_TIMER_OFF);
      enabled = _enableReversed ? LOW : HIGH ;
      digitalWrite(_enablePin, enabled);
      sendOSC();
      return;
    case ENT_RUNNING:
      // idle_timer.set(ATM_TIMER_OFF);
      // moving_timer.set(MOVING_TIMEOUT_DURATION);
      return;
    case LP_RUNNING:
      tempStep = motor->getPosition();
      connectors[ON_ONCHANGEPOSITION].push(_currentStep);
      // if (tempStep != _currentStep){push( connectors, ON_ONCHANGEPOSITION, 0, _currentStep, 0 );} ;
      _currentStep =  tempStep;
      updateLimitSwitch();
      sendOSC();
      return;
    case ENT_STOPPING:
      idle_timer.set(ATM_TIMER_OFF);
      moving_timer.set(ATM_TIMER_OFF);
      sendOSC();
      return;
    case LP_STOPPING:
      return;
    case ENT_EMERGENCY:
      idle_timer.set(ATM_TIMER_OFF);
      moving_timer.set(ATM_TIMER_OFF);
      controller->emergencyStop();
      sendOSC();
      return;
    case ENT_HOMING:
      // idle_timer.set(ATM_TIMER_OFF);
      // moving_timer.set(MOVING_TIMEOUT_DURATION);
      _limitType ? move(-2147483647) : trigger(EVT_STOP) ;
      sendOSC();
      return;
    case LP_HOMING:
      updateLimitSwitch();
      sendOSC();
      return;
    case EXT_HOMING:
      switch(next_trigger){
        case EVT_LIMIT_LOW:
          Serial.println("LOW");
          move(10);
          return;
        case EVT_LIMIT_HIGH:
          Serial.println("HIGH");
          move(-10);
          return;
      }

      return;
    case ENT_MOVE_TIMEOUT:
      idle_timer.set(ATM_TIMER_OFF);
      moving_timer.set(ATM_TIMER_OFF);
      emergency();
      return;
  }
}

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

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

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

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

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

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

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

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

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

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

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

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

Atm_Tstepper& Atm_Tstepper::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_Tstepper::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;
  }

}

int Atm_Tstepper::printPosition(){Serial.println(_currentStep);return 0;}

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

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


// Atm_Tstepper& Atm_Tstepper::move(long int stepRel ) {
//
//   // _motor.move(targetStep);
//   // _motor.run();
//   // Serial.println(_motor.distanceToGo());
//   // _motor.setMaxSpeed(5000);
//   // _motor.setAcceleration(6000);
//   trigger( EVT_GOTO );
//   return *this;
// }

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

/* Public event methods
 *
 */

Atm_Tstepper& Atm_Tstepper::homing_counter() {
  trigger( EVT_HOMING_COUNTER );
  return *this;
}

Atm_Tstepper& Atm_Tstepper::idle_timeout() {
  trigger( EVT_IDLE_TIMEOUT );
  return *this;
}

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

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

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

Atm_Tstepper& Atm_Tstepper::emergency() {
  controller->emergencyStop();
  trigger( EVT_EMERGENCY );
  return *this;
}

Atm_Tstepper& Atm_Tstepper::stop() {
  controller->stop();
  trigger( EVT_STOP );
  return *this;
}

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

Atm_Tstepper& Atm_Tstepper::move(long int stepRel) {
  _targetStep   += stepRel;
  motor->setTargetAbs(_targetStep);
  controller->moveAsync(*motor);
  enable();
  trigger( EVT_MOVE );
  return *this;
}

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

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

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

Atm_Tstepper& Atm_Tstepper::homing(int direction) {
  trigger( EVT_HOMING );
  return *this;
}

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

Atm_Tstepper& Atm_Tstepper::onOnchange( Machine& machine, int event ) {
  onPush( connectors, ON_ONCHANGE, 0, 1, 1, machine, event );
  return *this;
}

Atm_Tstepper& Atm_Tstepper::onOnchange( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_ONCHANGE, 0, 1, 1, callback, idx );
  return *this;
}

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

Atm_Tstepper& Atm_Tstepper::onOnchangeacceleration( Machine& machine, int event ) {
  onPush( connectors, ON_ONCHANGEACCELERATION, 0, 1, 1, machine, event );
  return *this;
}

Atm_Tstepper& Atm_Tstepper::onOnchangeacceleration( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_ONCHANGEACCELERATION, 0, 1, 1, callback, idx );
  return *this;
}

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

Atm_Tstepper& Atm_Tstepper::onOnchangeposition( Machine& machine, int event ) {
  onPush( connectors, ON_ONCHANGEPOSITION, 0, 1, 1, machine, event );
  return *this;
}

Atm_Tstepper& Atm_Tstepper::onOnchangeposition( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_ONCHANGEPOSITION, 0, 1, 1, callback, idx );
  return *this;
}

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

Atm_Tstepper& Atm_Tstepper::onOnchangestate( Machine& machine, int event ) {
  onPush( connectors, ON_ONCHANGESTATE, 0, 1, 1, machine, event );
  return *this;
}

Atm_Tstepper& Atm_Tstepper::onOnchangestate( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_ONCHANGESTATE, 0, 1, 1, callback, idx );
  return *this;
}

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

Atm_Tstepper& Atm_Tstepper::onOnhighlimit( Machine& machine, int event ) {
  onPush( connectors, ON_ONHIGHLIMIT, 0, 1, 1, machine, event );
  return *this;
}

Atm_Tstepper& Atm_Tstepper::onOnhighlimit( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_ONHIGHLIMIT, 0, 1, 1, callback, idx );
  return *this;
}

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

Atm_Tstepper& Atm_Tstepper::onOnhomingcounterout( Machine& machine, int event ) {
  onPush( connectors, ON_ONHOMINGCOUNTEROUT, 0, 1, 1, machine, event );
  return *this;
}

Atm_Tstepper& Atm_Tstepper::onOnhomingcounterout( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_ONHOMINGCOUNTEROUT, 0, 1, 1, callback, idx );
  return *this;
}

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

Atm_Tstepper& Atm_Tstepper::onOnhomingtimeout( Machine& machine, int event ) {
  onPush( connectors, ON_ONHOMINGTIMEOUT, 0, 1, 1, machine, event );
  return *this;
}

Atm_Tstepper& Atm_Tstepper::onOnhomingtimeout( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_ONHOMINGTIMEOUT, 0, 1, 1, callback, idx );
  return *this;
}

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

Atm_Tstepper& Atm_Tstepper::onOnidletimeout( Machine& machine, int event ) {
  onPush( connectors, ON_ONIDLETIMEOUT, 0, 1, 1, machine, event );
  return *this;
}

Atm_Tstepper& Atm_Tstepper::onOnidletimeout( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_ONIDLETIMEOUT, 0, 1, 1, callback, idx );
  return *this;
}

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

Atm_Tstepper& Atm_Tstepper::onOnlowlimit( Machine& machine, int event ) {
  onPush( connectors, ON_ONLOWLIMIT, 0, 1, 1, machine, event );
  return *this;
}

Atm_Tstepper& Atm_Tstepper::onOnlowlimit( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_ONLOWLIMIT, 0, 1, 1, callback, idx );
  return *this;
}

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

Atm_Tstepper& Atm_Tstepper::onOnmovetimeout( Machine& machine, int event ) {
  onPush( connectors, ON_ONMOVETIMEOUT, 0, 1, 1, machine, event );
  return *this;
}

Atm_Tstepper& Atm_Tstepper::onOnmovetimeout( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_ONMOVETIMEOUT, 0, 1, 1, callback, idx );
  return *this;
}

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

Atm_Tstepper& Atm_Tstepper::onSpeed( Machine& machine, int event ) {
  onPush( connectors, ON_SPEED, 0, 1, 1, machine, event );
  return *this;
}

Atm_Tstepper& Atm_Tstepper::onSpeed( atm_cb_push_t callback, int idx ) {
  onPush( connectors, ON_SPEED, 0, 1, 1, callback, idx );
  return *this;
}

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

Atm_Tstepper& Atm_Tstepper::trace( Stream & stream ) {
  Machine::setTrace( &stream, atm_serial_debug::trace,
    "TSTEPPER\0EVT_HOMING_COUNTER\0EVT_IDLE_TIMEOUT\0EVT_MOVE_TIMEOUT\0EVT_LIMIT_LOW\0EVT_LIMIT_HIGH\0EVT_EMERGENCY\0EVT_STOP\0EVT_ONTARGET\0EVT_MOVE\0EVT_DISABLE\0EVT_ENABLE\0ELSE\0DISABLED\0ENABLED\0RUNNING\0STOPPING\0EMERGENCY\0HOMING\0MOVE_TIMEOUT\0HOMING_COUNTEROUT" );
  return *this;
}