#include <Wire.h>
#include <LiquidCrystal_I2C.h>

#define PIN_POT 2
#define PIN_1A 0
#define PIN_1B 1
#define PIN_2A 4
#define PIN_2B 5
#define PIN_EN 3

#define LCD_LENGTH 16
#define MOTOR_STEPS 200

static int cycle_1[] = {1, 0, 0, 1};
static int cycle_2[] = {1, 1, 0, 0};

static int p = 0;
static int p_ref = 0;

LiquidCrystal_I2C lcd(0x3F, LCD_LENGTH, 2);
static char lcd_txt[LCD_LENGTH];

int sign(int value) {
  return int((value>0)-(value<0));
}

ISR(TCB0_INT_vect) {
  int i;
  int s = sign(p_ref-p);

  // step
  p += s;
  i = p%4;
  digitalWrite(PIN_1A, cycle_1[i]);
  digitalWrite(PIN_1B, 1-cycle_1[i]);
  digitalWrite(PIN_2A, cycle_2[i]);
  digitalWrite(PIN_2B, 1-cycle_2[i]);

  // reset interrupt
  TCB0.INTFLAGS = TCB_CAPT_bm;
}

void update_pot() {
  // measure potentiometer
  int v = analogRead(PIN_POT);
  p_ref = map(v, 0, 1023, 0, MOTOR_STEPS);
}

void update_screen() {
  int i=0;
  lcd.clear();

  // reference position
  sprintf(lcd_txt, "ref:%03d", p_ref);
  lcd.setCursor(0, 0);
  lcd.print(lcd_txt);

  // loading bar
  for (i=0;i<LCD_LENGTH;i++) {
    if (p_ref > (i*MOTOR_STEPS/LCD_LENGTH)) {
      lcd_txt[i] = '=';
    } else {
      lcd_txt[i] = ' ';
    }
  }
  lcd.setCursor(0, 1);
  lcd.print(lcd_txt);
}

void setup() {
  // setup pins
  pinMode(PIN_POT, INPUT);
  pinMode(PIN_1A, OUTPUT);
  pinMode(PIN_2A, OUTPUT);
  pinMode(PIN_1B, OUTPUT);
  pinMode(PIN_2B, OUTPUT);
  pinMode(PIN_EN, OUTPUT);
  digitalWrite(PIN_EN, HIGH);

  // LCD screen
  lcd.init();
  lcd.backlight();

  // init position
  update_pot();
  p = p_ref;

  // disable interrupts
  cli();
  // setup Timer B
  TCB0.CTRLA = TCB_CLKSEL_CLKTCA_gc;
  TCB0.CTRLB = TCB_CNTMODE_INT_gc;
  TCB0.CCMP = 0x0AFF;
  TCB0.INTCTRL = TCB_CAPTEI_bm;
  TCB0.CTRLA |= TCB_ENABLE_bm;
  // enable interrupts
  sei();
}

void loop() {
  int c;
  for (c=0;c<10;c++) {
    update_pot();
    delay(10);
  }
  update_screen();
}