#include "Atm_TeensyStep.h"


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

Atm_TeensyStep& Atm_TeensyStep::begin(Stepper & motorRef, StepControl & stepControlRef,
                  EthernetUDP& udpRef, OSCBundle& bndl, const char* address ) {
  // 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,     EXT_IDLE,             -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 );
  this-> motor =   &motorRef;
  this-> controller = &stepControlRef;
  _adress = address;
  this->_udpRef =   &udpRef;
  this->_bndl = &bndl ;
  return *this;
}

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

int Atm_TeensyStep::event( int id ) {
  switch ( id ) {
    case EVT_IDLE_TIMER:
      return 0;
    case EVT_ON_TARGET:
      //motor->get(targetStep);
      return _currentStep == _targetStep ;
      // 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_TeensyStep::action( int id ) {
  String strg = _adress;
  switch ( id ) {
    case ENT_IDLE:
      enable(0);
      return;
    case EXT_IDLE:
      enable(1);
      return;
    case ENT_READY:

      return;
    case ENT_RUNNING:

      // _controller.moveAsync(*_motor);
      return;
    case LP_RUNNING:
      // _motor.run();
      _currentStep = motor->getPosition();

      strg += "/step";
      _bndl->add(strg.c_str()).add(_currentStep);
      updateLimitSwitch();
      if(limitState[0] || limitState[1]) {emergencyStop();}
     // Serial.println(_motor.currentPosition());
      return;
    case ENT_STOPPING:
      return;
    case EXT_STOPPING:
      if(_isHoming && limitState[0]){motor->setPosition(0);}
      if(_isHoming && limitState[1]){maxStep = motor->getPosition();;}
      _isHoming=0;
      return;
  }
}

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

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

Atm_TeensyStep& Atm_TeensyStep::onOSC(OSCMessage& msg ){
  Serial.println("OSC");
  int patternOffset = msg.match(_adress) ;
  if(patternOffset){
    if(msg.fullMatch("/speedAcc", patternOffset)){
      motor->setMaxSpeed(msg.getInt(0));
      motor->setAcceleration(msg.getInt(1));
    }
    if(msg.fullMatch("/enable", patternOffset)){enable(msg.getInt(0));}
    if(msg.fullMatch("/home", patternOffset)){home(msg.getInt(0));}
    if(msg.fullMatch("/move", patternOffset)){move(msg.getInt(0));}
    if(msg.fullMatch("/moveTo", patternOffset)){moveTo(msg.getInt(0));}
    if(msg.fullMatch("/stop", patternOffset)){stop();}
    if(msg.fullMatch("/emergencyStop", patternOffset)){emergencyStop();}

    return *this;
  }
}

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

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

void Atm_TeensyStep::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;
  }

}

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

/* Public event methods
 *
 */
 Atm_TeensyStep& Atm_TeensyStep::setLimitPins( int limitPinLow){
   _limitPin[0] = limitPinLow;
   pinMode(_limitPin[0], INPUT);
   return *this;
 }

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

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

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

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

Atm_TeensyStep& Atm_TeensyStep::enable( bool enable ){
  enabled = enable ;
  enabled = _enableReversed ? !enabled : enabled ;
  digitalWrite(_enablePin, enabled);
  return *this;
}

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

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

Atm_TeensyStep& Atm_TeensyStep::home( bool limit ){
  _isHoming = 1 ;
  limit ? move(-2147483647) : move(2147483647) ; // move to very far away until hit
  return *this;
}

Atm_TeensyStep& Atm_TeensyStep::move(long int stepRel ) {
  _targetStep   += stepRel;
  motor->setTargetAbs(_targetStep);
  controller->moveAsync(*motor);

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

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

Atm_TeensyStep& Atm_TeensyStep::stop(){
  controller->stopAsync();
  //trigger( EVT_STOPPING );
  return *this;
}

Atm_TeensyStep& Atm_TeensyStep::emergencyStop(){
  controller->emergencyStop();
  //trigger( EVT_STOPPING );
  return *this;
}

//Atm_TeensyStep& Atm_TeensyStep::onOSC(OSCMessage& msg)

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

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

Atm_TeensyStep& Atm_TeensyStep::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_TeensyStep& Atm_TeensyStep::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;
}