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

#include <stdint.h>
#include "SEGGER_RTT.h"

#define PWMA_PORT GPIOC
#define PWMA_PIN  GPIO8
#define AIN1_PORT GPIOB
#define AIN1_PIN  GPIO8
#define AIN2_PORT GPIOB
#define AIN2_PIN  GPIO9

#define PWMB_PORT GPIOC
#define PWMB_PIN  GPIO9
#define BIN1_PORT GPIOC
#define BIN1_PIN  GPIO6
#define BIN2_PORT GPIOC
#define BIN2_PIN  GPIO5

static void gpio_setup(void) {
    rcc_periph_clock_enable(RCC_GPIOA);
    rcc_periph_clock_enable(RCC_GPIOB);
    rcc_periph_clock_enable(RCC_GPIOC);

    // LED LD2
    gpio_mode_setup(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO5);

    // USER Button B1
    gpio_mode_setup(GPIOC, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO13);

    // PWMA
    gpio_mode_setup(PWMA_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, PWMA_PIN);
    // AIN1
    gpio_mode_setup(AIN1_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, AIN1_PIN);
    // AIN2
    gpio_mode_setup(AIN2_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, AIN2_PIN);

    // PWMB
    gpio_mode_setup(PWMB_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, PWMB_PIN);
    // BIN1
    gpio_mode_setup(BIN1_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, BIN1_PIN);
    // BIN2
    gpio_mode_setup(BIN2_PORT, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, BIN2_PIN);
}

typedef enum {
    MOTOR_BRAKE,
    MOTOR_CCW,
    MOTOR_CW,
    MOTOR_STOP,
} motor_state_t;

static void drive_b(motor_state_t state) {
    switch (state) {
        case MOTOR_BRAKE:
            gpio_set(PWMB_PORT, PWMB_PIN);
            gpio_set(BIN1_PORT, BIN1_PIN);
            gpio_set(BIN2_PORT, BIN2_PIN);
            break;
        case MOTOR_CCW:
            gpio_set(PWMB_PORT, PWMB_PIN);
            gpio_clear(BIN1_PORT, BIN1_PIN);
            gpio_set(BIN2_PORT, BIN2_PIN);
            break;
        case MOTOR_CW:
            gpio_set(PWMB_PORT, PWMB_PIN);
            gpio_set(BIN1_PORT, BIN1_PIN);
            gpio_clear(BIN2_PORT, BIN2_PIN);
        case MOTOR_STOP:
            gpio_set(PWMB_PORT, PWMB_PIN);
            gpio_clear(BIN1_PORT, BIN1_PIN);
            gpio_clear(BIN2_PORT, BIN2_PIN);
    }
}

int main(void) {

    SEGGER_RTT_printf(0, "Startup\n");

    rcc_clock_setup_pll(&rcc_hse_8mhz_3v3[RCC_CLOCK_3V3_84MHZ]);

    gpio_setup();

    while (1) {
        if (gpio_get(GPIOC, GPIO13)) {
            gpio_toggle(GPIOA, GPIO5);
            drive_b(MOTOR_STOP);
            SEGGER_RTT_printf(0, "BRAKE\n");

        } else {
            drive_b(MOTOR_CCW);
            SEGGER_RTT_printf(0, "DRIVE\n");
        }

        for (int i = 0; i < 100000; i++) {	/* Wait a bit. */
            __asm__("nop");
        }
    }
}