#include <libopencm3/stm32/rcc.h>
#include <libopencm3/stm32/gpio.h>
#include <libopencm3/stm32/usart.h>

#include <libopencm3/cm3/systick.h>

enum direction {
  DIR_CLOCKWISE,
  DIR_COUNTERCLOCKWISE
};

volatile struct {
  bool active;
  bool done;
  int step_count;
  enum direction dir;
  int counter;
  int increment;
  int threshold;
  int phase;
} stepper = {
  true, false, 1000000, DIR_CLOCKWISE, 0, 1, 2, 0
};

volatile struct {
  bool active;
  bool done;
  enum direction dir;
  int count;
  int tocount;
} brushed = {
  false, false, DIR_CLOCKWISE, 0, 0
};

int main(void) {
	rcc_periph_clock_enable(RCC_GPIOA);
	rcc_periph_clock_enable(RCC_GPIOB);

	rcc_periph_clock_enable(RCC_USART2);

  // setup PA5-7 and PB1 for the stepper motor gates
  gpio_clear(GPIOA, GPIO5|GPIO6|GPIO7);
  gpio_clear(GPIOB, GPIO1);
  gpio_mode_setup(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO5);
  gpio_mode_setup(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO6);
  gpio_mode_setup(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO7);
  gpio_mode_setup(GPIOB, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO1);

  // setup PA2/3 for USART2_RX and USART2_TX
  gpio_mode_setup(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO2|GPIO3);
  gpio_set_af(GPIOA, GPIO_AF4, GPIO2|GPIO3);

  // setup USART2 stuff
  usart_set_baudrate(USART2, 115200);
	usart_set_databits(USART2, 8);
	usart_set_stopbits(USART2, USART_STOPBITS_1);
	usart_set_mode(USART2, USART_MODE_TX_RX);
	usart_set_parity(USART2, USART_PARITY_NONE);
	usart_set_flow_control(USART2, USART_FLOWCONTROL_NONE);
  usart_enable(USART2);

#if 0
  // if I need more speed than the internal 2 MHz
  // setup PA0 as a clock input
  rcc_bypass_enable(RCC_HSE);
  rcc_osc_on(RCC_HSE);

  rcc_wait_for_osc_ready(RCC_HSE);
  rcc_set_sysclk_source(RCC_HSE);
#endif

  // setup systick interrupt
  systick_interrupt_disable();
  systick_counter_disable();

  systick_set_clocksource(STK_CSR_CLKSOURCE_AHB);
  systick_set_reload(rcc_ahb_frequency/1000-1);
  systick_clear();

  systick_interrupt_enable();
  systick_counter_enable();

  int32_t val = 0;

  while (1) {
    if (stepper.done) {
      stepper.done = false;
      usart_send_blocking(USART2, 's');
    }
    if (brushed.done) {
      brushed.done = false;
      usart_send_blocking(USART2, 'r');
    }


    if ((USART_ISR(USART2) & USART_ISR_RXNE) != 0) {
      // TODO process byte
      uint8_t rx = usart_recv(USART2);

      // if it's a hexadecimal echo back and append it to val
      if ((rx >= '0') && (rx <= '9')) {
        usart_send_blocking(USART2, rx); // echo back
        val <<= 4;
        val |= (rx - '0');
      } else if ((rx >= 'a') && (rx <= 'f')) {
        usart_send_blocking(USART2, rx); // echo back
        val <<= 4;
        val |= (rx - 'a' + 10);
      } else if ((rx >= 'A') && (rx <= 'F')) {
        usart_send_blocking(USART2, rx); // echo back
        val <<= 4;
        val |= (rx - 'A' + 10);
      } else {
        switch (rx) {
        case 's': // get status
          if (stepper.active) {
            usart_send_blocking(USART2, '!');
          } else {
            usart_send_blocking(USART2, '@');
          }
          if (brushed.active) {
            usart_send_blocking(USART2, '#');
          } else {
            usart_send_blocking(USART2, '$');
          }
          break;
        case 'w': // start stepper
          usart_send_blocking(USART2, '!');
          stepper.active = true;
          break;
        case 'W': // stop stepper
          stepper.active = false;
          usart_send_blocking(USART2, '@');
          break;
        case 'r': // start brushed motor
          usart_send_blocking(USART2, '#');
          brushed.count = 0;
          brushed.active = true;
          break;
        case 'R': // stop brushed motor
          usart_send_blocking(USART2, '$');
          brushed.count = 0;
          brushed.active = false;
          break;
        case 't': // read stepper counter
          // TODO
          break;
        case 'T': // write stepper counter
          // TODO
          break;
        case 'y': // read stepper increment
          // TODO
          break;
        case 'Y': // write stepper increment
          // TODO
          break;
        case 'u': // read stepper threshold
          // TODO
          break;
        case 'U': // write stepper threshold
          // TODO
          break;
        case 'i': // set stepper direction to clockwise
          stepper.dir = DIR_CLOCKWISE;
          usart_send_blocking(USART2, 'i');
          break;
        case 'I': // set stepper direction to counterclockwise
          stepper.dir = DIR_COUNTERCLOCKWISE;
          usart_send_blocking(USART2, 'I');
          break;
        case 'o': // write brushed counter
          // TODO
          break;
        case 'O': // read brushed counter
          // TODO
          break;
        case 'p': // set brushed direction to clockwise
          brushed.dir = DIR_CLOCKWISE;
          usart_send_blocking(USART2, 'p');
          break;
        case 'P': // set brushed direction to counterclockwise
          brushed.dir = DIR_COUNTERCLOCKWISE;
          usart_send_blocking(USART2, 'P');
          break;
        }
      }
    }
  }
}

const struct {
    bool active;
    bool positive;
} phase_lut[][2] = {
  {{true, true}, {false, true}}, // A+
  //{{true, true}, {true, true}}, // A+, B+
  {{false, true}, {true, true}}, // B+
  //{{true, false}, {true, true}}, // A-, B+
  {{true, false}, {false, true}}, // A-
  //{{true, false}, {true, false}}, // A-, B-
  {{false, false}, {true, false}}, // B-
  //{{true, true}, {true, false}}, // A+, B-
};


void sys_tick_handler(void);
void sys_tick_handler(void) {
  if (!stepper.active) {
    gpio_clear(GPIOA, GPIO5|GPIO6|GPIO7);
    gpio_clear(GPIOB, GPIO1);
  } else {
    stepper.counter += stepper.increment;
    if (stepper.counter >= stepper.threshold) {
      --stepper.step_count;
      stepper.counter -= stepper.threshold;
      if (stepper.dir == DIR_CLOCKWISE) {
        ++stepper.phase;
      } else {
        --stepper.phase;
      }
      if (stepper.phase < 0) {
        stepper.phase = 3;
      }
      if (stepper.phase >= 4) {
        stepper.phase = 0;
      }
    }
    if (stepper.step_count <= 0) {
      stepper.counter = 0;
      stepper.active = false;
      stepper.done = true;
      // power down stepper motor; TODO maybe lock its position?
      gpio_clear(GPIOA, GPIO5|GPIO6|GPIO7);
      gpio_clear(GPIOB, GPIO1);
    } else {
      // write to GPIO pins
      // TODO maybe use a set of OR/AND masks instead? this works but probably
      // isn't constant time
      if (phase_lut[stepper.phase][0].active) {
        if (phase_lut[stepper.phase][0].positive) {
          gpio_clear(GPIOA, GPIO6);
          gpio_set(GPIOB, GPIO1);
        } else {
          gpio_set(GPIOA, GPIO6);
          gpio_clear(GPIOB, GPIO1);
        }
      } else {
        gpio_clear(GPIOA, GPIO6);
        gpio_clear(GPIOB, GPIO1);
      }
      if (phase_lut[stepper.phase][1].active) {
        if (phase_lut[stepper.phase][1].positive) {
          gpio_clear(GPIOA, GPIO7);
          gpio_set(GPIOA, GPIO5);
        } else {
          gpio_set(GPIOA, GPIO7);
          gpio_clear(GPIOA, GPIO5);
        }
      } else {
        gpio_clear(GPIOA, GPIO7);
        gpio_clear(GPIOA, GPIO5);
      }
    }
  }

  if (brushed.active) {
    if (brushed.count == 0) {
      // TODO toggle pins
    }
    ++brushed.count;
    if (brushed.count >= brushed.tocount) {
      // TODO toggle pins
      brushed.count = 0;
      brushed.active = false;
      brushed.done = true;
    }
  }

  // TODO check limit switches
}