#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 = 100;
int stall_open = 100; // from 0 to 255 (higher value gives you higher sensitivity)
int current_open = 1200; // Current value in mA
int current_close = 1200; // 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;




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");
 

}

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

  stepper.enableOutputs();
  stepper.setAcceleration(MOVE_ACCEL);
  stepper.setMaxSpeed(MOVE_VELOCITY);
  driver.TCOOLTHRS(0xFFFFF);
  //TCOOLS = (3089838.00*pow(float(MOVE_VELOCITY),-1.00161534))*1.5;
  
  
  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(stepper.targetPosition());
    while(stepper.currentPosition() != stepper.targetPosition())
    {
      stepper.run();
    }
  }
  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(stepper.targetPosition());
    while(stepper.currentPosition() != stepper.targetPosition())
    {
      stepper.run();
    }
  }
  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(4);
driver.blank_time(24);
driver.microsteps(motor_microsteps);
driver.TCOOLTHRS(0xFFFFF);
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();
    }
  }
  


}