// -*- mode: c++; -*-
//
// Startracker drives a NEMA17 for compensating the earth rotation
// during long exposure photography.  Original code based on JjRobots
// (see below), but everything has been rewritten since the begining
// of this project.
//
// This was tested with an arduino nano.
// Board wiring (and PCB) are available along with this code.
// See https://framagit.org/marc/startracker
//
// Initial header :
// STARTRACKER MOTOR CONTROL: STEPPER MOTOR CONTROL FOR JJROBOTS POV DISPLAY
// This code is designed for JJROBOTS arDusplay Stepper Motor Control board
// Author: JJROBOTS.COM (Jose Julio & Juan Pedro)
// Licence: GNU GPL
// Stepper : NEMA17
// Driver : A4988 or DRV8825
// Microstepping : configured to 1/16
// Arduino board: Pro micro (leonardo equivalent)

#include <DRV8825.h>

#include "config.h"

// using a 200-step motor (most common)
// pins used are DIR, STEP, MS1, MS2, MS3 in that order
DRV8825 stepper(200, dir_pin, step_pin,
		enable_pin,
		m0_pin, m1_pin, m2_pin);

#include <Switch.h>

#if DEBUG
#define dprint(x) Serial.println(x)
#else
#define dprint(x)
#endif

#if WEAK_DEBUG
#define dwprint(x) Serial.println(x)
#else
#define dwprint(x)
#endif

#if MODERATE_DEBUG
#define mprint(x) Serial.println(x)
#else
#define mprint(x)
#endif

//other info needed:
//ratio between the large gear and the small one=0.2549

// Science here !
#include "teeth_config.h"

static const float nr_teeth_small = CONFIG_TEETH_SMALL; // 13.0
static const float nr_teeth_big = CONFIG_TEETH_BIG; // 51.0

// Use immediate value. Using symbolic values leads to incorrect value.
static const float earth_rot_speed_rad_msec = 7.272205e-8; //2*PI / (1440*60);

//static const float coef = 2*PI*axis_hinge_dist_mm * nr_teeth_big / (bolt_thread_mm * nr_teeth_small);

static const unsigned int microstepping_div = 32;
static const unsigned int nr_steps = 200 * microstepping_div;

static const float stepper_gear_rad_per_step = (2*PI) / nr_steps;

#if MODERATE_DEBUG || DEBUG
static unsigned int loop_count = 0;
#endif

struct rot_state_t {
  unsigned long elapsed_time_millis;
  float stepper_gear_rot_rad = 0;
};

static float get_expected_stepper_rot(rot_state_t *s);

// The init function simply skips time from the 0 (hinge is closed) to
// the initial state configured by the initial_rod_deploy value.  When
// hinge is opening, it's really simply skipping time. When hinge is
// closing, there is a change in reference for the angle.
//
// If hinge opening mode, angle here is simply the angle formed by the
// moving plate and the fixed plate, constrained by the initial rod
// deployment.
//
// In hinge closing mode, angle is computed the same way, but the
// reference is still 0 with the earth rotation growing, so we take
// the complementary (PI - a).
static void init_rot_state(struct rot_state_t *state) {
  if (hinge_opening)
    state->elapsed_time_millis = atan(initial_rod_deploy / axis_hinge_dist_mm) / earth_rot_speed_rad_msec;
  else
    state->elapsed_time_millis = (PI - atan(initial_rod_deploy / axis_hinge_dist_mm)) / earth_rot_speed_rad_msec;

  state->stepper_gear_rot_rad = get_expected_stepper_rot(state);
}

static const unsigned int btn1_pin = 8;
Switch button1Switch = Switch(btn1_pin);

// static const unsigned int btn2_pin = 5;
// static const unsigned int btn3_pin = 6;
static const unsigned int end_stop_pin = 10;

#define ENABLE_LED_BLINK (0)

#define USE_ACTIVE_WAIT (1)
static const int use_active_wait = USE_ACTIVE_WAIT;
static const long active_threshold = 10;

static struct {
  unsigned long period;
  unsigned long deadline;
  unsigned long remain;
  unsigned int  expired;
} active_timer;

static unsigned long global_period_msec = 100;//(2*60+51)*1000;

static enum control_state_e {
  STARTUP = -1,
  IDLE = 0,
  RUN  = 1,
  RUN_OR_RESET = 2,
  RESET_POSITION = 3,
} control_state = STARTUP;

#define DUMP(v) do {				\
    Serial.print(#v " ");			\
    Serial.println(v, 10);			\
  } while(0)

#if DEBUG || MODERATE_DEBUG
static void debug_long(rot_state_t *s){
  const unsigned long ellapsed_in_msec = s->elapsed_time_millis;
  DUMP(ellapsed_in_msec);
  DUMP(earth_rot_speed_rad_msec);
  DUMP(axis_hinge_dist_mm);
  DUMP(nr_teeth_big);
  DUMP(nr_teeth_small);
  DUMP(bolt_thread_mm);
  DUMP(PI);
}
#endif

// Returns the value in radian the motor should have turned to reach
// the current value of earth rotation since the beggining.
static float
get_expected_stepper_rot(rot_state_t *s)
{
  float a = earth_rot_speed_rad_msec * s->elapsed_time_millis /* ellapsed_in_sec */;

  if (hinge_opening)
    a = PI - a;

  const float r = tan(a)
    * axis_hinge_dist_mm
    * 2 * PI
    * nr_teeth_big
    / (bolt_thread_mm * nr_teeth_small);

#if DEBUG || MODERATE_DEBUG

#if MODERATE_DEBUG
  if (!(loop_count % 100)) {
#endif
    debug_long(s);

    Serial.print("Angle final: ");
    Serial.println(r);

#if MODERATE_DEBUG
  }
#endif /* MODERATE_DEBUG */
#endif /* DEBUG */
  return r;
}

// Returns the length of the rod really deployed.
static float
get_rod_deploy (rot_state_t *s)
{
  float a = s->elapsed_time_millis * earth_rot_speed_rad_msec;

  if (!hinge_opening)
    a = PI - a;

  return tan(a) * axis_hinge_dist_mm;
}

// Returns the number of steps and the direction needed to reach given
// rotation angle in radian.
static int
get_step_number(rot_state_t *s, float expected_rotation)
{
  const float angle_diff = expected_rotation - s->stepper_gear_rot_rad;
  const float fsteps = angle_diff / stepper_gear_rad_per_step;
  const int steps = floor(fsteps);

#if DEBUG
  Serial.print("current rot:");
  Serial.println(s->stepper_gear_rot_rad, 6);
  Serial.print("diff :");
  Serial.print(angle_diff, 6);
  Serial.print(" needed steps : ");
  Serial.print(steps);
  Serial.print(" with fsteps: ");
  Serial.println(fsteps);

  const float round_per_minutes = (60000/global_period_msec)*fsteps*stepper_gear_rad_per_step;
  Serial.print("RAD per minutes (stepper) : ");
  Serial.println(round_per_minutes, 6);
  const float round_per_minutes_drive = round_per_minutes * (nr_teeth_small / nr_teeth_big);
  Serial.print("RAD per minutes (drive) : ");
  Serial.println(round_per_minutes_drive, 6);
#endif
  return steps;
}

// Make the motor move to desired angle
static void
set_stepper_rotation(rot_state_t *s, float angle)
{
  const int needed_steps = get_step_number(s, angle);
  if (stepper_direction) {
    stepper.move(needed_steps);
  } else {
    stepper.move(-needed_steps);
  }

  s->stepper_gear_rot_rad += needed_steps * stepper_gear_rad_per_step;
}

static void
start_timer(unsigned long period)
{
#if DEBUG
  Serial.println("start timer");
#endif

  if (use_active_wait) {
    // be careful: remain is signed and period is unsigned.
    active_timer.period = active_timer.remain = period;
    active_timer.deadline = millis() + period;
  }

#if DEBUG == 2
  Serial.print("Start Timer: ");
  Serial.println(active_timer.remain);
#endif

}

static void
stop_timer(void)
{
  if (use_active_wait){
    active_timer.deadline = active_timer.remain = 0;
  }

  //disable timer interrupt
  // disable timer
}

static void
handle_active_timer(void)
{

#if DEBUG == 2
  Serial.print("Timer: ");
  Serial.println(active_timer.remain);
#endif
  unsigned long current_time = millis();

  if (active_timer.deadline){
    active_timer.remain = active_timer.deadline - current_time;
    if (active_timer.remain > active_timer.deadline) {
      // unsigned underflow
      active_timer.remain = 0;
    }
  } else {
    return;
  }

  if (active_timer.remain <= active_threshold) {
    active_timer.remain = active_timer.period;
    active_timer.deadline = current_time + active_timer.period;
    active_timer.expired++;

#if DEBUG || MODERATE_DEBUG
    loop_count++;
#endif

#if DEBUG
    Serial.println("Timer expired");
    Serial.println(active_timer.expired);
    Serial.println(active_timer.remain);
    Serial.println(loop_count);
#endif
  }
}

#if WEAK_DEBUG
static bool new_state = true;

#define STATE(name)				\
  if (new_state) {				\
    Serial.print("ENTERING STATE: ");		\
    Serial.println(#name);			\
    new_state = false;				\
  }
#define NEXT_STATE(name)			\
  do {						\
    new_state = true;				\
    control_state = name;			\
  } while(0)

#else
#define STATE(name)
#define NEXT_STATE(name)			\
  control_state = name;

#endif

// this needs to be reset
static struct rot_state_t rot_state;

static void
control_automata(void)
{

#if DEBUG == 2
  Serial.println("Automata step...");
#endif

  button1Switch.poll();

  switch(control_state){
  case STARTUP: // Should happen only once
    STATE(STARTUP);
    NEXT_STATE(IDLE);
    break;

  case IDLE:
    STATE(IDLE);
    if (button1Switch.pushed()) {
      NEXT_STATE(RUN_OR_RESET);
      dwprint("Pushed: IDLE => RUN_OR_RESET");
    }

    break;

  case RUN_OR_RESET: {
    STATE(RUN_OR_RESET);
    unsigned long reset_delay = millis() + 500;
    NEXT_STATE(RUN);

    // stepper will be used in both exit states
    stepper.enable();

    while(millis() < reset_delay) {
      button1Switch.poll();

      if (button1Switch.pushed()) {
	NEXT_STATE(RESET_POSITION);
	dwprint("Pushed RUN_OR_RESET => RESET_POSITION");
	break;
      }
    }

    if (control_state == RUN) { // means not pushed during waiting time
      dwprint("NOT Pushed RUN_OR_RESET => RUN");
      start_timer(global_period_msec);
    }
    break;
  }
  case RUN:
    STATE(RUN);
    if (button1Switch.pushed()) {
      dwprint("Short press RUN => IDLE");
      stop_timer();
      stepper.disable();
      NEXT_STATE(IDLE);
    } else if (get_rod_deploy(&rot_state) < min_rod_deploy) {
      dwprint("Min value for hinge reached, RUN => IDLE");
      stop_timer();
      stepper.disable();
      NEXT_STATE(IDLE);
    } else if (active_timer.expired) {
      active_timer.expired--;
      // emit_motor_step();
      // step_motor();
      rot_state.elapsed_time_millis += active_timer.period;
      const float expected_rot = get_expected_stepper_rot(&rot_state);
      set_stepper_rotation(&rot_state, expected_rot);

#if ENABLE_LED_BLINK
      blink_led();
#endif
    }

    break;

  case RESET_POSITION: {
    STATE(RESET_POSITION);
    unsigned long debounce_reset = millis();
    int reset_done = 0;

    while(!reset_done) {
      if (stepper_direction) {
	stepper.move(-1);
      } else {
	stepper.move(1);
      }

      int level = digitalRead(end_stop_pin);
      if ( level == HIGH) {
	debounce_reset = millis();
      } else {
	if (millis() - debounce_reset > 50) {
	  reset_done = 1;
	}
      }
    }
    init_rot_state(&rot_state);
    NEXT_STATE(IDLE);
    stepper.disable();
    dwprint("Finished RESET, => IDLE");
    break;
  }
  }

#if DEBUG == 2
  Serial.println("End of Automata step...");
#endif

}

void
setup()
{
  // debug output
  Serial.begin(serial_speed);

  dwprint("Serial setup");

  // Set target motor RPM to 1RPM
  stepper.setRPM(30);
  // Set full speed mode (microstepping also works for smoother hand movement
  stepper.setMicrostep(microstepping_div);
  dwprint("Microstepping is");
  dwprint(microstepping_div);

  stepper.disable();

  // Setup PIN as GPIO output
  pinMode(led_pin, OUTPUT);    // LED pin

  // Button input with pullups enable
  //  pinMode(btn1_pin, INPUT);
  // pinMode(btn2_pin, INPUT_PULLUP);
  // pinMode(btn3_pin, INPUT_PULLUP);
  pinMode(end_stop_pin, INPUT_PULLUP);

  // Initial setup for motor driver
  // digitalWrite(led_pin, HIGH);
  // delay(200);    // Initial delay
  // digitalWrite(led_pin, LOW);

  init_rot_state(&rot_state);

  dwprint("Setup finished, starting loop");
}

void
loop(void)
{
  if (use_active_wait)
    handle_active_timer();

  control_automata();
}