//com 30

#define PIN_A1 6
#define PIN_A2 7
#define PIN_A_PWM 27

#define PIN_B1 28
#define PIN_B2 4
#define PIN_B_PWM 29

#define BITMASK_MARKER 0b10000000 // 0b - binary (1,2,4...)
#define BITMASK_DATA 0b01111111

#define BUFFER_LENGTH 32


uint8_t rx_buffer[BUFFER_LENGTH];
int rx_buffer_len = 0;

int16_t pos = 0;
uint8_t addr = 0; // this is the first stepper

bool lookup_a[] = {true, true, false, false};
bool lookup_b[] = {true, false, false, true};

void setup() { 
  Serial.begin(115200);
  Serial1.begin(115200);
  pinMode(PIN_A1, OUTPUT);
  pinMode(PIN_A2, OUTPUT);
  pinMode(PIN_A_PWM, OUTPUT);
  pinMode(PIN_B1, OUTPUT);
  pinMode(PIN_B2, OUTPUT);
  pinMode(PIN_B_PWM, OUTPUT);

  analogWrite(PIN_A_PWM, 120);
  analogWrite(PIN_B_PWM, 120);
  update_output();
}

void decode_message() {
  if (rx_buffer_len != 6) {
    // invalid, give up
    return;
  }
  int msg_addr = rx_buffer[0] & BITMASK_DATA;
  int16_t msg_pos = ((int16_t)(rx_buffer[1] & BITMASK_DATA) << 9) | ((int16_t)(rx_buffer[2] & BITMASK_DATA) << 2);
  msg_pos /= 4;
  uint16_t msg_delta = ((uint16_t)(rx_buffer[3] & BITMASK_DATA) << 7) | ((uint16_t)(rx_buffer[4] & BITMASK_DATA));
  Serial.print("Message: ");
  Serial.print(msg_addr);
  Serial.print("|");
  Serial.print(msg_pos);
  Serial.print("|");
  Serial.print(msg_delta);
  Serial.println("");
  
  if (msg_addr == addr) {
    Serial.println("Moving...");
    move_to(msg_pos, msg_delta);
    Serial.println(pos);
  }
}

void loop() {
  if (Serial1.available()) {
    char c = Serial1.read();
    rx_buffer[rx_buffer_len] = c;
    rx_buffer_len++;
    if (rx_buffer_len == BUFFER_LENGTH) {
      // overflowing buffer, throw everything out
      rx_buffer_len = 0;
    } else if (c == '\x00') {
      // decode message
      decode_message();
      rx_buffer_len = 0;
    }
  }
}

inline void update_output() {
  uint8_t lookup_index = pos % 4;
  digitalWrite(PIN_A1, lookup_a[lookup_index]);
  digitalWrite(PIN_A2, !lookup_a[lookup_index]);
  digitalWrite(PIN_B1, lookup_b[lookup_index]);
  digitalWrite(PIN_B2, !lookup_b[lookup_index]);
}

void move_to(int16_t msg_pos, uint16_t msg_delta) {
  int16_t pos_delta = msg_pos - pos;

  if (pos_delta == 0) {
    return;
  }

  int16_t sgn = pos_delta > 0 ? 1 : -1;

  while (pos != msg_pos) {
    pos += sgn;
    update_output();
    delayMicroseconds(msg_delta);
  }
}