#include "Arduino.h"
#include "config.h"
#include "FocheggiatoreLib.h"
#include <TMC2130Stepper.h>

/*
 * CONSTRUCTOR
 */
Focheggiatore::Focheggiatore(int MOT_driverEnable, int MOT_dirPin, int MOT_stepPin, int MOT_selectPin, int MOT_switchFocPri, int MOT_switchFocSec, int MOT_switchHomeFoc, int MOT_MOSI, int MOT_MISO, int MOT_SCK )
{
      this->driverEnable = MOT_driverEnable;
      this->dirPin = MOT_dirPin;
      this->stepPin = MOT_stepPin;
      this->selectPin = MOT_selectPin;
      this->switchFocPri = MOT_switchFocPri;
      this->switchFocSec = MOT_switchFocSec;
      this->switchHome = MOT_switchHomeFoc;
      this->mosi = MOT_MOSI;
      this->miso = MOT_MISO;
      this->sck = MOT_SCK;
}




/*
 *  INIZIALIZZAZIONE
 */
void Focheggiatore::begin()
{
      // Ottengo i dati dalla eeprom
      getEeprom();

      // Inizializzo le variabili di servizio
      appo                  = 0;
      count                 = 0;
      timeH                 = 0;
      timeL                 = 0;
      timeDelay             = 50;
      limit                 = false;
      backlashCounter       = backlash;
      oldDir                = 0;
      infinity              = 0;
      currentMicros         = 0;
      currentMillis         = 0;
      rampTimeMicro         = 0;
      rampTimeRatio         = 0;
      targetStep            = 0;
      oldError              = 1;
      statusError           = 1;
      checkErrorPrec        = 0;
      status                = STATUS_STOP;

      TMC2130Stepper driver = TMC2130Stepper(driverEnable, dirPin, stepPin, selectPin, PIN_MOSI, PIN_MISO, PIN_SCK);

      // Inizializzo i PIN
      pinMode(switchFocPri, INPUT);
      pinMode(switchFocSec, INPUT);
      pinMode(switchHome, INPUT);

      // Inizializzo il Driver Amis
      driver.begin();
      driver.reset();
      driver.rms_current( CONF_FOC_CURRENT_LIMIT );
      driver.stealthChop(1);
      driver.microsteps( 1 );

      digitalWrite(driverEnable, LOW);

      Serial.print("DRV_STATUS=0b");
      Serial.println(driver.DRV_STATUS(), BIN);
}



/*
 * MOVIMENTAZIONE MOTORE
 */
void Focheggiatore::move( char dirLoc, long int stepOut, int inf = 0 )
{

        initRampData();

        if( dirLoc == KEY_FORWARD){
          homePosition = false;
          status = KEY_FORWARD;
          setStepDirection(1);
          dir = 1;
        }
        else if ( dirLoc == KEY_BACKWARD){
          homePosition = false;
          status = KEY_BACKWARD;
          setStepDirection(0);
          dir = 0;
        }
        else if( dirLoc == KEY_HOME ){
          homePosition = true;
          status = KEY_BACKWARD;
          infinity = 1;
          setStepDirection(1);
          dir = 1;
          Serial.println( "target step: " );
          Serial.println( targetStep );
          targetStep = 1000 * CONF_FOC_MICROSTEP;
          return;
        }

        if( inf ){
          infinity = 1;
        }else{
          infinity = 0;
        }
        Serial.println( "target step: " );
        Serial.println( targetStep );
        targetStep = CONF_FOC_MICROSTEP * stepOut;

}

/*
 * MOTOR STOP
 */
void  Focheggiatore::stop()
{
      limit = false;
      oldDir = dir;
      targetStep = 0;
      status = STATUS_STOP;
      infinity = 0;
      digitalWrite(stepPin, LOW);
      Serial.println( "stop" );
}





/*
 * MOVIMENTAZIONE STEP
 */
void Focheggiatore::singleSteps ()
{
        currentMillis = millis();
        currentMicros = micros();
        // Se sono fermo non faccio nulla
        if( status == STATUS_STOP ){
          return;
        }


        // Gestisco la fine del movimento. Se non infinto mi fermo altrimenti rigenero gli step traget
        if( targetStep == 0 && infinity ){
          targetStep = 1000 * CONF_FOC_MICROSTEP;
          return;
        }else if( targetStep == 0 && !infinity ){
          stop();
        }


        // Controllo il driver
        if( timeH == 0 && timeL == 0){
          //sbi(PORTB, stepPin);
          digitalWrite(stepPin, HIGH);
          timeH = currentMicros;
          timeL = 0;
          return;
        }
        if( timeH!=0 && currentMicros - timeH > CONF_FOC_LOW_MICRO_TIME ){
            //cbi(PORTB, stepPin);
            digitalWrite(stepPin, LOW);
            timeH = 0;
            timeL = currentMicros;
            return;
        }
       if( timeL!=0 && (currentMicros - timeL > speedDelay  ) ){
            timeH = 0;
            timeL = 0;
            targetStep--;
            appo++;
            if(appo == CONF_FOC_MICROSTEP){

              if( dir == 1 ){
                steps--;
              }
              else{
                steps++;
              }

              appo = 0;


            return;
            }

        }
}






/*
 * SET DIRECTION
 */
void Focheggiatore::setStepDirection( bool dirLoc )
{
      Serial.println( "direzione: " );
      Serial.println( dirLoc );
      driver.shaft_dir(dirLoc);
}