#include "project.h"

#define P1_N GPIO12
#define P1_N_PORT GPIOB
#define P1_P GPIO13
#define P1_P_PORT GPIOB
#define P2_N GPIO14
#define P2_N_PORT GPIOB
#define P2_P GPIO15
#define P2_P_PORT GPIOB


int gauge_target;
static int gauge_current;
static unsigned zeroing = 1000;

#define GUAGE_N_STATE 6
static unsigned gauge_state;


static inline unsigned
cw (unsigned v)
{
  v += GUAGE_N_STATE - 1;

  if (v >= GUAGE_N_STATE)
    v -= GUAGE_N_STATE;

  return v;
}


static inline unsigned
ccw (unsigned v)
{
  v += 1;

  if (v >= GUAGE_N_STATE)
    v -= GUAGE_N_STATE;

  return v;
}

static inline void
p1 (int i)
{
  if (i < 0) {
    SET (P1_N);
    CLEAR (P1_P);
  } else if (i > 0) {
    CLEAR (P1_N);
    SET (P1_P);
  } else {
    CLEAR (P1_N);
    CLEAR (P1_P);
  }
}

static inline void
p2 (int i)
{
  if (i < 0) {
    SET (P2_N);
    CLEAR (P2_P);
  } else if (i > 0) {
    CLEAR (P2_N);
    SET (P2_P);
  } else {
    CLEAR (P2_N);
    CLEAR (P2_P);
  }
}


void
gauge_ticker (void)
{

  if (zeroing) {
    zeroing--;
    gauge_state = ccw (gauge_state);
    gauge_current = -10;
  } else {

    if (gauge_current < gauge_target) {
      gauge_state = cw (gauge_state);
      gauge_current++;
    } else if (gauge_current > gauge_target) {
      gauge_state = ccw (gauge_state);
      gauge_current--;
    }
  }


  switch (gauge_state) {
  case 0:
    p1 (1);
    p2 (1);
    break;

  case 1:
    p1 (1);
    p2 (0);
    break;

  case 2:
    p1 (0);
    p2 (-1);
    break;

  case 3:
    p1 (-1);
    p2 (-1);
    break;

  case 4:
    p1 (-1);
    p2 (0);
    break;

  case 5:
    p1 (0);
    p2 (1);
  }

}

void
gauge_test (void)
{
  char buf[8];
#if 0
  uint32_t now = dwt_read_cycle_counter();  /*This wraps every 59.9 seconds or so */

  now &= 0x3fffffff;            /* now wraps every 15s */
  now = now / (0x3fffffff / 810);

  gauge_target = now;
#else

  if (gauge_target == gauge_current)
    gauge_target ^= 900;

#endif


  sprintf (buf, "%6d", gauge_current);
  font8x8_put_str (vram_1, buf, 80, 24);
}




void
gauge_init (void)
{
  MAP_OUTPUT_PP (P1_N);
  MAP_OUTPUT_PP (P1_P);
  MAP_OUTPUT_PP (P2_N);
  MAP_OUTPUT_PP (P2_P);
}