#include "Motor_Controler.h"
#include "SettingsAndConstants.h"

Motor_Controler *instance;

static void IRAM_ATTR static_counter_interrupt_handler(void)
{
  instance->Counter_interrupt_handler();
}

Motor_Controler::Motor_Controler(int mosi, int miso, int sck,
                                 int en_s, int dir_s, int step_s, int cs_s, int microsteps_s, int current_s, int schop_s, double full_step_angle_s, int teeth_count,
                                 int en_w, int dir_w, int step_w, int cs_w, int microsteps_w, int current_w, int schop_w, double full_step_angle_w, double lead_per_rotate,
                                 double RSense, double hcur_s, double hcur_w,
                                 int hole_sensor, int home_sensor, double init_position) :
                                 micsteps_s(microsteps_s),
                                 micsteps_w(microsteps_w),
                                 en_step_pin(en_s),
                                 hole_sensor_pin(hole_sensor),
                                 home_sensor_pin(home_sensor),
                                 width_s_pin(step_w),
                                 step_s_pin(step_s),
                                 initPosition(init_position),
                                 step_driver(en_s, dir_s, step_s, cs_s, mosi, miso, sck),
                                 width_driver(en_w, dir_w, step_w, cs_w, mosi, miso, sck)
{
  // Init instance of this class
  instance = this;

  // Step motor init
  /*
   * Parameter initialization of tape move stepper motor driver
   */

  // Width motor init
  /*
   * Parameter initialization of width limiter stepper motor driver
   */

  // Init sensor inputs
  #ifndef BYPASS_SENSOR_CHECKS
  Serial.println("Sensors initialization");
  pinMode(hole_sensor, INPUT);
  pinMode(home_sensor, INPUT);
  #endif

  // Calculate move constants
  /*
   * Calculate constants for precision moving of stepper motors
   */
}

void Motor_Controler::Counter_interrupt_handler(void)
{
  hole_counter++;
}

int Motor_Controler::GetHoleCount(void)
{
  int tmp = hole_counter;
  hole_counter = 0;
  return tmp;
}

bool Motor_Controler::_checkTempFlag(TMC2130Stepper driver)
{
  return driver.getOTPW();
}

void Motor_Controler::_enableCounter()
{
  attachInterrupt(digitalPinToInterrupt(hole_sensor_pin), static_counter_interrupt_handler, COUNTER_MODE);
  hole_counter = 0;
}

void Motor_Controler::_disableCounter()
{
  detachInterrupt(digitalPinToInterrupt(hole_sensor_pin));
  hole_counter = 0;
}

void Motor_Controler::_step(int step_pin, int move_speed, int num_of_steps, int ramp_steps = NULL, float ramp_amount = NULL)
{
  /*
   * Move stepper motor by given number of steps
   * If ramps are set then speed will increse at start and decrese at end of stepping cycle
   */
}

bool Motor_Controler::SwitchMotorSleep(void)
{
  digitalWrite(en_step_pin, !digitalRead(en_step_pin));
}

bool Motor_Controler::HomeWidth(int move_speed)
{
  // If bypass of sensors then this function is disabled
  #ifdef BYPASS_SENSOR_CHECKS
  return false;
  #endif
  
  bool init_state = (digitalRead(home_sensor_pin) == LOW);
  unsigned int steps = 0;
  // Calculate maximum number of steps (60mm is more than feeder can achieve)
  unsigned int max_steps = 60.0 / width_per_width_step;

  if (init_state)
  {
    // If we are in home sensor
    if (INVERT_WIDTH_DIR)
      width_driver.shaft_dir(false);
    else
      width_driver.shaft_dir(true);

    while (digitalRead(home_sensor_pin) == LOW)
    {
      _step(width_s_pin, move_speed, 1);
      steps++;
      if (steps > max_steps) return false;
    }

    delay(200);

    if (INVERT_WIDTH_DIR)
      width_driver.shaft_dir(true);
    else
      width_driver.shaft_dir(false);

    while (digitalRead(home_sensor_pin) == HIGH)
    {
      _step(width_s_pin, move_speed, 1);
      steps++;
      if (steps > max_steps) return false;
    }

    delay(200);

  }
  else
  {
    // If we are not in home sensor
    if (INVERT_WIDTH_DIR)
      width_driver.shaft_dir(true);
    else
      width_driver.shaft_dir(false);

    while (digitalRead(home_sensor_pin) == HIGH)
    {
      _step(width_s_pin, move_speed, 1);
      steps++;
      if (steps > max_steps) return false;
    }

    delay(200);
  }
  
  width_position = initPosition;
  return true;
}

bool Motor_Controler::Step(bool forward, int move_speed, int pitch)
{
  if (!step_driver.isEnabled()) digitalWrite(en_step_pin, LOW);
  
  if (INVERT_STEP_DIR)
    step_driver.shaft_dir(!forward);
  else
    step_driver.shaft_dir(forward);

  if (_checkTempFlag(step_driver))
  {
    Serial.println("SDTF Set!");
    return false;
  }

  _enableCounter();

  if (TAPE_MOVE_USE_RAMP)
  {
    _step(step_s_pin, move_speed, pitch * steps_per_move_tooth, TAPE_MOVE_SPEED_RAMP_FULL_STEPS * step_microsteps, TAPE_MOVE_RAMP_DELAY_COEF);
  }
  else
  {
    _step(step_s_pin, move_speed, pitch * steps_per_move_tooth);
  }
  
  Serial.print("Hole counter: ");
  Serial.println(hole_counter);
  _disableCounter();

  #ifndef BYPASS_SENSOR_CHECKS
  if ((pitch - MIN_HOLE_OFFSET) <= hole_counter && hole_counter <= (pitch + MAX_HOLE_OFFSET))
  {
    return true;
  }
  else
  {
    return false;
  }
  #endif

  return true;
}

bool Motor_Controler::RollOutTape(int pitch, int move_speed)
{
  #ifdef BYPASS_SENSOR_CHECKS
  return false;
  #endif
  
  if (!step_driver.isEnabled()) digitalWrite(en_step_pin, LOW);
  
  if (INVERT_STEP_DIR)
    step_driver.shaft_dir(false);
  else
    step_driver.shaft_dir(true);

  // step_driver.stealthChop(0);

  _enableCounter();
  while (true)
  {
    _step(step_s_pin, move_speed, (pitch * 2) * steps_per_move_tooth);
    if (GetHoleCount() == 0) break;
  }
  _disableCounter();

  _step(step_s_pin, move_speed, 20 * steps_per_move_tooth);
  digitalWrite(en_step_pin, HIGH);
  
  return true;
}

bool Motor_Controler::WasHomed(void)
{
  return width_position != -1;
}

bool Motor_Controler::SetWidth(double pos, int move_speed)
{
  if (width_position == -1) return false;
  double dif = pos - width_position;
  Serial.print("Width pos diff: ");
  Serial.println(dif);
  
  if (dif == 0.0) return true;
  width_position += dif;

  if (dif > 0)
  {
    if (INVERT_WIDTH_DIR)
      width_driver.shaft_dir(true);
    else
      width_driver.shaft_dir(false);
  }
  else
  {
    dif = -dif;
    if (INVERT_WIDTH_DIR)
      width_driver.shaft_dir(false);
    else
      width_driver.shaft_dir(true);
  }

  int sc = dif / width_per_width_step;

  if (TAPE_WIDTH_USE_RAMP)
  {
    _step(width_s_pin, move_speed, sc, TAPE_WIDTH_SPEED_RAMP_FULL_STEPS * width_microsteps, TAPE_WIDTH_RAMP_DELAY_COEF);
  }
  else
  {
    _step(width_s_pin, move_speed, sc);
  }
  
  return true;
}

void Motor_Controler::TrimStep(bool forward, int move_speed)
{
  if (!step_driver.isEnabled()) digitalWrite(en_step_pin, LOW);
  
  if (INVERT_STEP_DIR)
    step_driver.shaft_dir(!forward);
  else
    step_driver.shaft_dir(forward);
  
  _step(step_s_pin, move_speed, TAPE_TRIM_STEPS);
}

void Motor_Controler::TrimWidth(bool add, int move_speed)
{
  #ifndef BYPASS_SENSOR_CHECKS
  if (width_position == -1) return;

  double dif = WIDTH_TRIM_STEPS * width_per_width_step;
  double expWidth;
  
  if (add)
  {
    expWidth = width_position + dif;
  }
  else
  {
    expWidth = width_position - dif;
  }

  if (expWidth < MIN_WIDTH || expWidth > MAX_WIDTH) return;
  #endif
  
  if (INVERT_WIDTH_DIR)
    width_driver.shaft_dir(add);
  else
    width_driver.shaft_dir(!add);

  _step(width_s_pin, move_speed, WIDTH_TRIM_STEPS);
  
  #ifndef BYPASS_SENSOR_CHECKS
  width_position = expWidth;
  #endif
}

void Motor_Controler::ShowParams(void)
{
  Serial.println("\nMotor driver parameters");
  Serial.print("Steps per tooth: ");
  Serial.println(steps_per_move_tooth);
  Serial.print("Width per step: ");
  Serial.println(width_per_width_step, 10);
}

double Motor_Controler::GetPosition()
{
  return width_position;
}

bool Motor_Controler::Test_Con()
{
  int stRes = step_driver.test_connection();
  int wRes = width_driver.test_connection();

  if (stRes == 0 && wRes == 0) return true;
  return false;
}
