#include "project.h"


static unsigned motor_pos[HANDS];
static unsigned off[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

#define ADJUST_CCW	GPIO0
#define ADJUST_CCW_PORT	GPIOC
#define ADJUST_CW	GPIO1
#define ADJUST_CW_PORT	GPIOC



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

  if (!m) return;

  c^=d;
  d^=c;
  c^=d;

#if 0
a^=c;
c^=a;
a^=c;

b^=d;
d^=b;
b^=d;
#endif


  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;


#if 0
  // full step
  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;
  }
#endif
#if 0
// half step
coils (m, !! ((s + 1) & 4), !! ((s + 6) & 4), !! ((s + 4) & 4), !! ((s + 7) & 4));
#endif
#if 1
//60deg phase

switch(s%6) {
case 0:
coils(m,0,0,1,0);
break;
case 1:
coils(m,1,0,1,0);
break;
case 2:
coils(m,1,0,0,0);
break;
case 3:
coils(m,0,0,0,1);
break;
case 4:
coils(m,0,1,0,1);
break;
case 5:
coils(m,0,1,0,0);
break;
}
#endif



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


}



#define ADJ_P 2500


void motor_tick (void)
{
  static uint32_t last,blast;
  unsigned i, d;
  static unsigned adjusting = ADJ_P; /*5 seconds*/
  unsigned hp[2];



  if ((ticks - last) < 2) return;
  last=ticks;


  if ((ticks - blast) > 100) {

  if (!GET(ADJUST_CCW))  {
	adjusting=ADJ_P;

	motor_pos[1]+=MOTOR_PHASES;
	motor_pos[1]%=MOTOR_STEPS;
  }


  if (!GET(ADJUST_CW))  {
	adjusting=ADJ_P;
	
	motor_pos[1]+=MOTOR_STEPS-MOTOR_PHASES;
	motor_pos[1]%=MOTOR_STEPS;
  }
  blast=ticks;

  }

	


  if (adjusting || !hands_ready) {
	if (adjusting) adjusting--;
	hp[0]=0;
	hp[1]=0;
  } else {
  	hp[0]=hands_pos[0];
  	hp[1]=hands_pos[1];
  }

  //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;
    d+=hp[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);

  MAP_INPUT_PU(ADJUST_CCW);
  MAP_INPUT_PU(ADJUST_CW);


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

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


}