#include "project.h"


static unsigned motor_pos[HANDS];

#define M1_A  GPIO11
#define M1_A_PORT GPIOB
#define M1_B  GPIO12
#define M1_B_PORT GPIOB
#define M1_C  GPIO13
#define M1_C_PORT GPIOB
#define M1_D  GPIO14
#define M1_D_PORT GPIOB


static void  coils (unsigned m, int a, int b, int c, int d)
{

  if (!m) return;

  if (a)
    SET (M1_A);
  else
    CLEAR (M1_A);

  if (b)
    SET (M1_B);
  else
    CLEAR (M1_B);

  if (c)
    SET (M1_C);
  else
    CLEAR (M1_C);

  if (d)
    SET (M1_D);
  else
    CLEAR (M1_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;


  switch (s & 3) {
  case  0:
    coils (m, 0, 1, 0, 1);
    break;

  case 1:
    coils (m, 0, 1, 1, 0);
    break;

  case 2:
    coils (m, 1, 0, 1, 0);
    break;

  case 3:
    coils (m, 1, 0, 0, 1);
    break;
  }



  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;
  static unsigned wiggle = MOTOR_STEPS;



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

  last = ticks;

  if (wiggle) {
    step (1, 1);
    wiggle--;
    return;
  }

  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)
{

  MAP_OUTPUT_PP (M1_A);
  MAP_OUTPUT_PP (M1_B);
  MAP_OUTPUT_PP (M1_C);
  MAP_OUTPUT_PP (M1_D);

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

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


}