#include "project.h"


static unsigned motor_pos[HANDS];
static unsigned off[HANDS];

#ifdef HBRIDGE

#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

#else

#define M1_DIR		GPIO0
#define M1_DIR_PORT	GPIOB
#define M1_STEP		GPIO1
#define M1_STEP_PORT	GPIOB
#define M1_EN		GPIO2
#define M1_EN_PORT	GPIOB

#define M2_DIR		GPIO10
#define M2_DIR_PORT	GPIOC
#define M2_STEP		GPIO11
#define M2_STEP_PORT	GPIOC
#define M2_EN		GPIO12
#define M2_EN_PORT	GPIOC


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



#ifdef HBRIDGE
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);
}
#endif

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

  s += MOTOR_STEPS + d;
  s %= MOTOR_STEPS;


#ifdef HBRIDGE
#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

#else

if (d<0) {
if (!m) {
	SET(M1_DIR);
	SET(M1_STEP);
	delay_us(1);
	CLEAR(M1_STEP);
} else {
	SET(M2_DIR);
	SET(M2_STEP);
	delay_us(1);
	CLEAR(M2_STEP);
}
} else if (d>0) {
if (!m) {
	CLEAR(M1_DIR);
	SET(M1_STEP);
	delay_us(1);
	CLEAR(M1_STEP);
} else {
	CLEAR(M2_DIR);
	SET(M2_STEP);
	delay_us(1);
	CLEAR(M2_STEP);
}
}

#endif


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


}




void motor_tick (void)
{
  static uint32_t last,blast;
  unsigned i, d;
  static unsigned adjusting =0;
  static unsigned zero=8000;
  unsigned hp[2];

  static unsigned s;





  //if ((ticks - last) < 1) return;
  //last=ticks;

  if ((ticks - blast) > 1000) {

  if (!GET(ADJUST_CCW))  {
	adjusting=8000;
  }


  if (!GET(ADJUST_CW))  {
	zero=8000;
  }
  blast=ticks;
  }

#ifndef HBRIDGE
   if (adjusting) {
	adjusting--;

	if (adjusting) {
	SET(M1_EN);
	SET(M2_EN);
	} else {
	CLEAR(M1_EN);
	CLEAR(M2_EN);
	}

	motor_pos[0]=motor_pos[1]=0;
	step(0,0);
	step(1,0);

	return;
    }
#endif

		

  if (zero || !hands_ready) {
	if (zero) zero--;
	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]);


#if 0
{
unsigned static r=0;
r++;
r%=6000;

if (r<3000) {
hp[0]=0;
} 
}
#endif

{ unsigned static s=0;


if (!s) {
	printf("%4d %4d hp[0]=%6d mp[0]=%6d hp[1]=%6d mp[1]=%6d\r\n",
		zero,adjusting, hp[0],motor_pos[0],hp[1],motor_pos[1]);
}




s++;
s%=500;
}




  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_INPUT_PU(ADJUST_CCW);
  MAP_INPUT_PU(ADJUST_CW);

#ifdef HBRIDGE
  MAP_OUTPUT_PP (M1_A);
  MAP_OUTPUT_PP (M1_B);
  MAP_OUTPUT_PP (M1_C);
  MAP_OUTPUT_PP (M1_D);


#else

  MAP_OUTPUT_PP (M1_EN);
  MAP_OUTPUT_PP (M1_DIR);
  MAP_OUTPUT_PP (M1_STEP);

  MAP_OUTPUT_PP (M2_EN);
  MAP_OUTPUT_PP (M2_DIR);
  MAP_OUTPUT_PP (M2_STEP);

  SET(M1_EN);
  CLEAR(M1_STEP);
  CLEAR(M1_DIR);
  delay_us(2);
  CLEAR(M1_EN);


  SET(M2_EN);
  CLEAR(M2_STEP);
  CLEAR(M2_DIR);
  delay_us(2);
  CLEAR(M2_EN);
#endif



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

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

}