#include "Atm_stepper.h"

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

Atm_stepper& Atm_stepper::begin(int stepPin, int dirPin) {
  // clang-format off
  const static state_t state_table[] PROGMEM = {
    /*                 ON_ENTER     ON_LOOP       ON_EXIT  EVT_IDLE_TIMER  EVT_ON_TARGET  EVT_GOTO  ELSE */
    /*     IDLE */     ENT_IDLE,         -1,           -1,             -1,            -1,  RUNNING,   -1,
    /*    READY */    ENT_READY,         -1,           -1,           IDLE,            -1,  RUNNING,   -1,
    /*  RUNNING */  ENT_RUNNING, LP_RUNNING,           -1,             -1,      STOPPING,  RUNNING,   -1,
    /* STOPPING */ ENT_STOPPING,         -1, EXT_STOPPING,             -1,         READY,       -1,   -1,
  };
  // clang-format on
  Machine::begin( state_table, ELSE );
  AccelStepper _motor (1, stepPin, dirPin);
  // Stepper _motor(stepPin, dirPin );
  // StepControl _controller;
  // _motor.setMaxSpeed(20000);
  // _motor.setAcceleration(1000);
  //_controller = new StepControl;
  pinMode(0, OUTPUT);
  digitalWrite(0, HIGH);
  return *this;
}

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

int Atm_stepper::event( int id ) {
  switch ( id ) {
    case EVT_IDLE_TIMER:
      return 0;
    case EVT_ON_TARGET:

      return _motor.distanceToGo()== 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> );
 */

void Atm_stepper::action( int id ) {
  switch ( id ) {
    case ENT_IDLE:
      digitalWrite(0, LOW);
      return;
    case ENT_READY:
    digitalWrite(0, HIGH);
      return;
    case ENT_RUNNING:
      digitalWrite(0, HIGH);
      // _controller.moveAsync(*_motor);
      return;
    case LP_RUNNING:
      _motor.run();

     Serial.println(_motor.currentPosition());
      return;
    case ENT_STOPPING:
      return;
    case EXT_STOPPING:
      return;
  }
}

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

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

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

/* Public event methods
 *
 */

Atm_stepper& Atm_stepper::gotoStep(long int targetStep ) {
  // _motor->setTargetRel(targetStep);

  _motor.move(targetStep);
  _motor.run();
  Serial.println(_motor.distanceToGo());
  _motor.setMaxSpeed(5000);
  _motor.setAcceleration(6000);
  trigger( EVT_GOTO );
  return *this;
}

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

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

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

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

Atm_stepper& Atm_stepper::trace( Stream & stream ) {
  Machine::setTrace( &stream, atm_serial_debug::trace,
    "STEPPER\0EVT_IDLE_TIMER\0EVT_ON_TARGET\0EVT_GOTO\0ELSE\0IDLE\0READY\0RUNNING\0STOPPING" );
  return *this;
}