/*
 * Module for interfacing with Switec instrument steppers (and
 * similar devices). These are the steppers that are used in automotive
 * instrument panels and the like. Run off 5 volts at low current.
 *
 * Code inspired by:
 *
 * SwitecX25 Arduino Library
 *  Guy Carpenter, Clearwater Software - 2012
 *
 *  Licensed under the BSD2 license, see license.txt for details.
 *
 * NodeMcu integration by Philip Gladstone, N1DQ
 */

#include "platform.h"
#include <stdint.h>
#include <stddef.h>
#include <stdlib.h>
#include <stdio.h>
#include "task/task.h"
#include "driver/switec.h"
#include "ets_sys.h"
#include "os_type.h"
#include "osapi.h"
#include "hw_timer.h"
#include "user_interface.h"

#define N_STATES 6
//
// First pin passed to setup corresponds to bit 3
// On the motor, the pins are arranged
//
//    4           1
//
//    3           2
//
// The direction of rotation can be reversed by reordering the pins
//
// State  3 2 1 0  A B  Value
// 0      1 0 0 1  - -  0x9
// 1      0 0 0 1  . -  0x1
// 2      0 1 1 1  + .  0x7
// 3      0 1 1 0  + +  0x6
// 4      1 1 1 0  . +  0xE
// 5      1 0 0 0  - .  0x8
static const uint8_t stateMap[N_STATES] = {0x9, 0x1, 0x7, 0x6, 0xE, 0x8};

typedef struct {
  uint8_t  current_state;
  uint8_t  stopped;
  int8_t   dir;
  uint32_t mask;
  uint32_t pinstate[N_STATES];
  uint32_t next_time;
  int16_t  target_step;
  int16_t  current_step;
  uint16_t vel;
  uint16_t max_vel;
  uint16_t min_delay;
  task_handle_t task_number;
} DATA;

static DATA *data[SWITEC_CHANNEL_COUNT];
static volatile char timer_active;

#define MAXVEL 255

// Note that this has to be global so that the compiler does not
// put it into ROM.
uint8_t switec_accel_table[][2] = {
    {   20, 3000 >> 4},
    {   50, 1500 >> 4},
    {  100, 1000 >> 4},
    {  150,  800 >> 4},
    {  MAXVEL,  600 >> 4}
};

static void ICACHE_RAM_ATTR timer_interrupt(os_param_t);

#define TIMER_OWNER ((os_param_t) 'S')


// Just takes the channel number
int switec_close(uint32_t channel)
{
  if (channel >= sizeof(data) / sizeof(data[0])) {
    return -1;
  }

  DATA *d = data[channel];

  if (!d) {
    return 0;
  }

  if (!d->stopped) {
    return -1;
  }

  // Set pins as input
  gpio_output_set(0, 0, 0, d->mask);

  data[channel] = NULL;
  free(d);

  // See if there are any other channels active
  for (channel = 0; channel < sizeof(data)/sizeof(data[0]); channel++) {
    if (data[channel]) {
      break;
    }
  }

  // If not, then disable the interrupt
  if (channel >= sizeof(data) / sizeof(data[0])) {
    platform_hw_timer_close(TIMER_OWNER);
  }

  return 0;
}

static __attribute__((always_inline)) inline void write_io(DATA *d)
{
  uint32_t pin_state = d->pinstate[d->current_state];

  gpio_output_set(pin_state, d->mask & ~pin_state, 0, 0);
}

static __attribute__((always_inline)) inline  void step_up(DATA *d)
{
  d->current_step++;
  d->current_state = (d->current_state + 1) % N_STATES;
  write_io(d);
}

static __attribute__((always_inline)) inline  void step_down(DATA *d)
{
  d->current_step--;
  d->current_state = (d->current_state + N_STATES - 1) % N_STATES;
  write_io(d);
}

static void ICACHE_RAM_ATTR timer_interrupt(os_param_t p)
{
  // This function really is running at interrupt level with everything
  // else masked off. It should take as little time as necessary.
  //
  (void) p;

  int i;
  uint32_t delay = 0xffffffff;

  // Loop over the channels to figure out which one needs action
  for (i = 0; i < sizeof(data) / sizeof(data[0]); i++) {
    DATA *d = data[i];
    if (!d || d->stopped) {
      continue;
    }

    uint32_t now = system_get_time();
    if (now < d->next_time) {
      int need_to_wait = d->next_time - now;
      if (need_to_wait < delay) {
	delay = need_to_wait;
      }
      continue;
    }

    // This channel is past it's action time. Need to process it

    // Are we done yet?
    if (d->current_step == d->target_step && d->vel == 0) {
      d->stopped = 1;
      d->dir = 0;
      task_post_low(d->task_number, 0);
      continue;
    }

    // if stopped, determine direction
    if (d->vel == 0) {
      d->dir = d->current_step < d->target_step ? 1 : -1;
      // do not set to 0 or it could go negative in case 2 below
      d->vel = 1;
    }

    // Move the pointer by one step in the correct direction
    if (d->dir > 0) {
      step_up(d);
    } else {
      step_down(d);
    }

    // determine delta, number of steps in current direction to target.
    // may be negative if we are headed away from target
    int delta = d->dir > 0 ? d->target_step - d->current_step : d->current_step - d->target_step;

    if (delta > 0) {
      // case 1 : moving towards target (maybe under accel or decel)
      if (delta <= d->vel) {
	// time to declerate
	d->vel--;
      } else if (d->vel < d->max_vel) {
	// accelerating
	d->vel++;
      } else {
	// at full speed - stay there
      }
    } else {
      // case 2 : at or moving away from target (slow down!)
      d->vel--;
    }

    // vel now defines delay
    uint8_t row = 0;
    // this is why vel must not be greater than the last vel in the table.
    while (switec_accel_table[row][0] < d->vel) {
      row++;
    }

    uint32_t micro_delay = switec_accel_table[row][1] << 4;
    if (micro_delay < d->min_delay) {
      micro_delay = d->min_delay;
    }

    // Figure out when we next need to take action
    d->next_time = d->next_time + micro_delay;
    if (d->next_time < now) {
      d->next_time = now + micro_delay;
    }

    // Figure out how long to wait
    int need_to_wait = d->next_time - now;
    if (need_to_wait < delay) {
      delay = need_to_wait;
    }
  }

  if (delay < 1000000) {
    if (delay < 50) {
      delay = 50;
    }
    timer_active = 1;
    platform_hw_timer_arm_us(TIMER_OWNER, delay);
  } else {
    timer_active = 0;
  }
}


// The pin numbers are actual platform GPIO numbers
int switec_setup(uint32_t channel, int *pin, int max_deg_per_sec, task_handle_t task_number )
{
  if (channel >= sizeof(data) / sizeof(data[0])) {
    return -1;
  }

  if (data[channel]) {
    if (switec_close(channel)) {
      return -1;
    }
  }

  DATA *d = (DATA *) calloc(1, sizeof(DATA));
  if (!d) {
    return -1;
  }

  if (!data[0] && !data[1] && !data[2]) {
    // We need to stup the timer as no channel was active before
    // no autoreload
    if (!platform_hw_timer_init(TIMER_OWNER, FRC1_SOURCE, FALSE)) {
      // Failed to get the timer
      free(d);
      return -1;
    }
  }

  data[channel] = d;
  int i;

  for (i = 0; i < 4; i++) {
    // Build the mask for the pins to be output pins
    d->mask |= 1 << pin[i];

    int j;
    // Now build the hi states for the pins according to the 6 phases above
    for (j = 0; j < N_STATES; j++) {
      if (stateMap[j] & (1 << (3 - i))) {
        d->pinstate[j] |= 1 << pin[i];
      }
    }
  }

  d->max_vel = MAXVEL;
  if (max_deg_per_sec == 0) {
    max_deg_per_sec = 400;
  }
  d->min_delay = 1000000 / (3 * max_deg_per_sec);
  d->task_number = task_number;

#ifdef SWITEC_DEBUG
  for (i = 0; i < 4; i++) {
    printf("pin[%d]=%d\n", i, pin[i]);
  }

  printf("Mask=0x%x\n", d->mask);
  for (i = 0; i < N_STATES; i++) {
    printf("pinstate[%d]=0x%x\n", i, d->pinstate[i]);
  }
#endif

  // Set all pins as outputs
  gpio_output_set(0, 0, d->mask, 0);

  platform_hw_timer_set_func(TIMER_OWNER, timer_interrupt, 0);

  return 0;
}

// All this does is to assert that the current position is 0
int switec_reset(uint32_t channel)
{
  if (channel >= sizeof(data) / sizeof(data[0])) {
    return -1;
  }

  DATA *d = data[channel];

  if (!d || !d->stopped) {
    return -1;
  }

  d->current_step = d->target_step = 0;

  return 0;
}

// Just takes the channel number and the position
int switec_moveto(uint32_t channel, int pos)
{
  if (channel >= sizeof(data) / sizeof(data[0])) {
    return -1;
  }

  DATA *d = data[channel];

  if (!d) {
    return -1;
  }

  if (pos < 0) {
    // This ensures that we don't slam into the endstop
    d->max_vel = 50;
  } else {
    d->max_vel = MAXVEL;
  }

  d->target_step = pos;

  // If the pointer is not moving, setup so that we start it
  if (d->stopped) {
    // reset the timer to avoid possible time overflow giving spurious deltas
    d->next_time = system_get_time() + 1000;
    d->stopped = false;

    if (!timer_active) {
      timer_interrupt(0);
    }
  }

  return 0;
}

// Get the current position, direction and target position
int switec_getpos(uint32_t channel, int32_t *pos, int32_t *dir, int32_t *target)
{
  if (channel >= sizeof(data) / sizeof(data[0])) {
    return -1;
  }

  DATA *d = data[channel];

  if (!d) {
    return -1;
  }

  *pos = d->current_step;
  *dir = d->stopped ? 0 : d->dir;
  *target = d->target_step;

  return 0;
}