#include	"home.h"

/** \file
  \brief Homing routines
*/

#include <math.h>
#include	"dda.h"
#include	"dda_queue.h"
#include	"pinio.h"
#include	"gcode_parse.h"

// Check configuration.
#if defined X_MIN_PIN || defined X_MAX_PIN
  #ifndef SEARCH_FEEDRATE_X
    #error SEARCH_FEEDRATE_X undefined. It should be defined in config.h.
  #endif
  #ifndef ENDSTOP_CLEARANCE_X
    #error ENDSTOP_CLEARANCE_X undefined. It should be defined in config.h.
  #endif
#endif
#if defined Y_MIN_PIN || defined Y_MAX_PIN
  #ifndef SEARCH_FEEDRATE_Y
    #error SEARCH_FEEDRATE_Y undefined. It should be defined in config.h.
  #endif
  #ifndef ENDSTOP_CLEARANCE_Y
    #error ENDSTOP_CLEARANCE_Y undefined. It should be defined in config.h.
  #endif
#endif
#if defined Z_MIN_PIN || defined Z_MAX_PIN
  #ifndef SEARCH_FEEDRATE_Z
    #error SEARCH_FEEDRATE_Z undefined. It should be defined in config.h.
  #endif
  #ifndef ENDSTOP_CLEARANCE_Z
    #error ENDSTOP_CLEARANCE_Z undefined. It should be defined in config.h.
  #endif
#endif

// Calculate feedrates according to clearance and deceleration.
// For a description, see #define ENDSTOP_CLEARANCE_{XYZ} in config.h.
//   s = 1/2 * a * t^2; t = v / a  <==> v = sqrt(2 * a * s))
//   units: / 1000 for um -> mm; * 60 for mm/s -> mm/min
#ifdef ENDSTOP_CLEARANCE_X
  #define SEARCH_FAST_X (uint32_t)((double)60. * \
            sqrt((double)2 * ACCELERATION * ENDSTOP_CLEARANCE_X / 1000.))
#endif
#ifdef ENDSTOP_CLEARANCE_Y
  #define SEARCH_FAST_Y (uint32_t)((double)60. * \
            sqrt((double)2 * ACCELERATION * ENDSTOP_CLEARANCE_Y / 1000.))
#endif
#ifdef ENDSTOP_CLEARANCE_Z
  #define SEARCH_FAST_Z (uint32_t)((double)60. * \
            sqrt((double)2 * ACCELERATION * ENDSTOP_CLEARANCE_Z / 1000.))
#endif

uint32_t get_fast_feedrate(enum axis_e n);
uint32_t get_search_feedrate(enum axis_e n);
uint8_t get_endstop_check(enum axis_e n, int8_t dir);
void home_axis(enum axis_e n, int8_t dir);
void set_axis_home_position(enum axis_e n, int8_t dir);

/// home all 3 axes
void home() {
  #ifdef DEFINE_HOMING
    #undef DEFINE_HOMING
      #define DEFINE_HOMING(first, second, third) \
        { \
          home_##first(); \
          home_##second(); \
          home_##third(); \
        };
      #include "config_wrapper.h"    
    #undef DEFINE_HOMING
  #else
    home_x_negative();
    home_x_positive();

    home_y_negative();
    home_y_positive();

    home_z_negative();
    home_z_positive();
  #endif
}


/// find X MIN endstop
void home_x_negative() {
  #if defined X_MIN_PIN
    home_axis(X, -1);
  #endif
}

/// find X_MAX endstop
void home_x_positive() {
  #if defined X_MAX_PIN && ! defined X_MAX
    #warning X_MAX_PIN defined, but not X_MAX. home_x_positive() disabled.
  #endif
  #if defined X_MAX_PIN && defined X_MAX
    home_axis(X, 1);
  #endif
}

/// fund Y MIN endstop
void home_y_negative() {
  #if defined Y_MIN_PIN
    home_axis(Y, -1);
  #endif
}

/// find Y MAX endstop
void home_y_positive() {
  #if defined Y_MAX_PIN && ! defined Y_MAX
    #warning Y_MAX_PIN defined, but not Y_MAX. home_y_positive() disabled.
  #endif
  #if defined Y_MAX_PIN && defined Y_MAX
    home_axis(Y, 1);
  #endif
}

/// find Z MIN endstop
void home_z_negative() {
  #if defined Z_MIN_PIN
    home_axis(Z, -1);
  #endif
}

/// find Z MAX endstop
void home_z_positive() {
  #if defined Z_MAX_PIN && ! defined Z_MAX
    #warning Z_MAX_PIN defined, but not Z_MAX. home_z_positive() disabled.
  #endif
  #if defined Z_MAX_PIN && defined Z_MAX
    home_axis(Z, 1);
  #endif
}

uint32_t get_fast_feedrate(enum axis_e n) {
  uint32_t feedrate = 0;
  if (n == X)
    feedrate = SEARCH_FAST_X > SEARCH_FEEDRATE_X ? SEARCH_FAST_X : SEARCH_FEEDRATE_X;
  else if (n == Y)
    feedrate = SEARCH_FAST_Y > SEARCH_FEEDRATE_Y ? SEARCH_FAST_Y : SEARCH_FEEDRATE_Y;
  else if (n == Z)
    feedrate = SEARCH_FAST_Z > SEARCH_FEEDRATE_Z ? SEARCH_FAST_Z : SEARCH_FEEDRATE_Z;
  return feedrate;
}

uint32_t get_search_feedrate(enum axis_e n) {
  uint32_t feedrate = 0;
  if (n == X)
    feedrate = SEARCH_FAST_X > SEARCH_FEEDRATE_X ? SEARCH_FEEDRATE_X : 0;
  else if (n == Y)
    feedrate = SEARCH_FAST_Y > SEARCH_FEEDRATE_Y ? SEARCH_FEEDRATE_Y : 0;
  else if (n == Z)
    feedrate = SEARCH_FAST_Z > SEARCH_FEEDRATE_Z ? SEARCH_FEEDRATE_Z : 0;
  return feedrate;
}

uint8_t get_endstop_check(enum axis_e n, int8_t dir) {
  uint8_t endstop_check;
  if (dir < 0)
    endstop_check = 1 << (n * 2);
  else
    endstop_check = 1 << (n * 2 + 1);
  return endstop_check;
}

void home_axis(enum axis_e n, int8_t dir) {
  TARGET t = startpoint;
  startpoint.axis[n] = 0;
  uint8_t endstop_check = get_endstop_check(n, dir);

  t.axis[n] = dir * MAX_DELTA_UM;
  t.F = get_fast_feedrate(n);
  enqueue_home(&t, endstop_check, 1);

  uint32_t search_feedrate;
  search_feedrate = get_search_feedrate(n);
  if (search_feedrate) {
    // back off slowly
    t.axis[n] = 0;
    t.F = search_feedrate;
    enqueue_home(&t, endstop_check, 0);
  }

  queue_wait();
  set_axis_home_position(n, dir);
  dda_new_startpoint();
}

void set_axis_home_position(enum axis_e n, int8_t dir) {
  int32_t home_position = 0;
  if (dir < 0) {
    if (n == X) {
      #ifdef X_MIN
      home_position = (int32_t)(X_MIN * 1000);
      #endif
    }
    else if (n == Y) {
      #ifdef Y_MIN
      home_position = (int32_t)(Y_MIN * 1000);
      #endif
    }
    else if (n == Z) {
      #ifdef Z_MIN
      home_position = (int32_t)(Z_MIN * 1000);
      #endif
    }
  }
  else {
    if (n == X) {
      #ifdef X_MAX
      home_position = (int32_t)(X_MAX * 1000);
      #endif
    }
    else if (n == Y) {
      #ifdef Y_MAX
      home_position = (int32_t)(Y_MAX * 1000);
      #endif
    }
    else if (n == Z) {
      #ifdef Z_MAX
      home_position = (int32_t)(Z_MAX * 1000);
      #endif
    }
  }
  startpoint.axis[n] = next_target.target.axis[n] = home_position;
}