#include "project.h"


static unsigned motor_pos[HANDS];


static void  coils (unsigned m, int a, int b, int c, int d)
{
  printf ("Motor %d: %+d %+d\r\n", m, a - b, c - d);
}

static void step (unsigned m, int d)
{
  unsigned s = motor_pos[m];

  if (d < 0) d += MOTOR_STEPS;

  s += d;
  s %= MOTOR_STEPS;

  coils (m, !! ((s + 6) & 4), !! ((s + 1) & 4), !! ((s + 4) & 4), !! ((s + 7) & 4));

  if (!m)
    BKP_DR8 = motor_pos[0] = s;
  else
    BKP_DR9 = motor_pos[1] = s;


}





void motor_tick (void)
{
  static uint32_t last;
  unsigned i, d;

  if ((ticks - last) < 10) return;

  last = ticks;

  if (!hands_ready) return;

  //printf("HANDS: %d -> %d    %d -> %d\r\n",hands_pos[0],motor_pos[0],hands_pos[1],motor_pos[1]);


  for (i = 0; i < HANDS; ++i) {
    d = MOTOR_STEPS + hands_pos[i];
    d -= motor_pos[i];
    d %= MOTOR_STEPS;

    if (d) {
      if (d < (MOTOR_STEPS / 2))
        step (i, 1);
      else
        step (i, -1);
    }
  }
}




void motor_init (void)
{

  motor_pos[0] = BKP_DR8;
  motor_pos[1] = BKP_DR9;

  step (0, 0);
  step (1, 0);

}