#include <Arduino.h>
#include <AccelStepper.h>
#include <TMCStepper.h>

#define STEP_PIN 6
#define DIR_PIN 5
#define ENABLE_PIN 7
#define SW_TX 16
#define SW_RX 17
#define STALLGUARD 2
#define R_SENSE 0.11f
#define DRIVER_ADDRESS 0b00

//VARIABLES

//#define MOVE 2
//#define STOP 3
int stall_close = 50;
int stall_open = 50; // from 0 to 255 (higher value gives you higher sensitivity)
int current_open = 800; // Current value in mA
int current_close = 800; // Current value in mA
float MOVE_VELOCITY = 5000; // Velocity of the motor
int CLOSE_POSITION = 1; // Reverse motor turn (1=TURN ON,2=TURN OFF)
float move_to_position = 0;
int max_steps = 300000; // Maximum position distance
int XACTUAL = 0;
int MOVE_ACCEL = 5000; // Acceleration
bool reverse_flag = false;
int TCOOLS = 300;
int motor_microsteps = 8;
//int command = 1;
int current_position;
int prom = 0;
bool stallguardDetected = false;
int pozice = 0;




AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);
//TMC2209Stepper driver(&Serial1, 0.11f , 0b00);
TMC2209Stepper driver(SW_RX, SW_TX, R_SENSE, DRIVER_ADDRESS); //zkus změnit na TMCStepper


//STALLGUARD PIN STOP MOTOR
void stalled_position()
{
  stepper.setAcceleration(200000);
  stepper.setMaxSpeed(0);
  
  stepper.moveTo(stepper.currentPosition());
  Serial.println("GUARD");
  if(move_to_position == 0) //když zadám 1
  {
  prom = 1;  
  }
  if(move_to_position == -27680) //když zadám 0
  {
  prom = 2;  
  }
  
 

}

//MOTOR RUNNING FUNCTION
void move_motor()
{
  move_to_position = stepper.targetPosition();
  XACTUAL = stepper.currentPosition();

  stepper.enableOutputs();
  stepper.setAcceleration(MOVE_ACCEL);
  stepper.setMaxSpeed(MOVE_VELOCITY);
  TCOOLS = (3089838.00*pow(float(MOVE_VELOCITY),-1.00161534))*1.5;
  driver.TCOOLTHRS(TCOOLS);
  
  
  
  Serial.println("skok");
  if(move_to_position > XACTUAL)
  {
    driver.SGTHRS(stall_open);
    driver.rms_current(current_open);
    Serial.print("aktual => 1: ");
    Serial.println(stepper.currentPosition());
    Serial.print("target => 1: ");
    Serial.println(move_to_position);
    Serial.print("prom: ");
    Serial.println(prom);
    
    while(stepper.currentPosition() != stepper.targetPosition())
    {
      stepper.run();
      if(digitalRead(STALLGUARD) == HIGH)
      {
        stalled_position();
      }
      //Serial.println(digitalRead(STALLGUARD));
    }

    if(prom == 2)
    {
      prom = 4;
    }
  }
  else if(move_to_position < XACTUAL)
  {
    driver.SGTHRS(stall_close);
    driver.rms_current(current_close);
    Serial.print("aktual => 0: ");
    Serial.println(stepper.currentPosition());
    Serial.print("target => 0: ");
    Serial.println(move_to_position);
    Serial.print("prom: ");
    Serial.println(prom);
    while(stepper.currentPosition() != stepper.targetPosition())
    {
      stepper.run();
      if(digitalRead(STALLGUARD) == HIGH)
      {
        stalled_position();
      }
      //Serial.println(digitalRead(STALLGUARD));
    }
    if(prom == 1)
    {
      prom = 3;
    }
  }
  else
  {
    Serial.println("ALREADY THERE!");
  }

  XACTUAL = stepper.currentPosition();
  //stepper.disableOutputs();
  Serial.println("MOTOR");
  //stepper.disableOutputs();
  
}


void setup() {

Serial.begin(250000);
while(!Serial);
Serial1.begin(115200);
driver.beginSerial(115200);

pinMode(ENABLE_PIN, OUTPUT);
pinMode(DIR_PIN, OUTPUT);
pinMode(STEP_PIN, OUTPUT);
pinMode(STALLGUARD, INPUT);

//INTERRUPT SETUP
//attachInterrupt(digitalPinToInterrupt(STALLGUARD), stalled_position, RISING);

//DRIVER SETUP

//driver.pdn_disable(true);
driver.begin();
//driver.TPWMTHRS(0);
driver.semin(5);
driver.semax(2);
driver.sedn(0b01);
driver.toff(3);
driver.blank_time(24);
driver.microsteps(motor_microsteps);
driver.TCOOLTHRS(0xFFFFF);
driver.hysteresis_start(4);
stepper.setAcceleration(MOVE_ACCEL);
stepper.setMaxSpeed(MOVE_VELOCITY);
//driver.SGTHRS(30);


//driver.en_spreadCycle(false);

//MOTOR SETUP
stepper.setEnablePin(ENABLE_PIN);
stepper.setPinsInverted(false, false, true);
stepper.setCurrentPosition(XACTUAL);
//stepper.disableOutputs();


if(CLOSE_POSITION==1)
{
  driver.shaft(true);
}
else
{
  driver.shaft(false);
}



}

void loop() {




  if (Serial.available()>0) 
  {
    int8_t read_byte = Serial.read();

    // Zpracujte příkaz, například pomocí podmínky if nebo switch
    if (read_byte == '0') 
    {
      stepper.moveTo(max_steps);
      move_motor(); 
    } 
    else if (read_byte == '1') 
    {
      stepper.moveTo(0);
      move_motor();
    }
  }
  if(prom == 1)
  {
      pozice = stepper.currentPosition()-1000;
      if(pozice < -27680)
      {
        pozice = -27680;
      }
      stepper.moveTo(pozice);
      move_motor();
  }
  if(prom == 3)
  {
    stepper.moveTo(0);
    prom = 0;
    move_motor();
    
  }

  if(prom == 2)
  {
      pozice = stepper.currentPosition()+1000;
      if(pozice > 0)
      {
        pozice = 0;
      }
      stepper.moveTo(pozice);
      move_motor();
  }
  if(prom == 4)
  {
    stepper.moveTo(max_steps);
    prom = 0;
    move_motor();
    
  }
  


}