#define DEVICE_ID 0xaa
#define KEEPALIVE_INTERVAL_MSEC 5000UL
#define DEBOUNCE_MILLIS 100

#define DRIVER_ENABLE 4
#define RECEIVER_nENABLE 2

#define SWITCH0 A3
#define SWITCH1 A2
#define SWITCH2 A1
#define SWITCH3 A0
#define SWITCH4 13
#define SWITCH5 12

#define BIT_SET(val, pos) (val |= (1 << pos))
#define BIT_CLEAR(val, pos) (val &= ~(1 << pos))

static const uint8_t input_pins[] = {SWITCH0, SWITCH1 , SWITCH2, SWITCH3, SWITCH4, SWITCH5};
static long input_last[] = {0, 0, 0, 0, 0, 0}; // Last millis() for each switch
static uint8_t last_bitmap = 0;
static long start_millis = 0, last_millis = 0;

typedef enum packet_type_t {
  PACKET_KEEPALIVE = 0,
  PACKET_INPUT, /* Discrete input button / switch event */
  PACKET_OUTPUT, /* Notify room controller that we triggered output */
  PACKET_SOLVED,
  PACKET_FAILED,
  PACKET_LOG,
} __attribute__((__packed__)) packet_type_t;

typedef struct pheader_t {
  packet_type_t p_type;
  uint8_t id;
} __attribute__((__packed__)) pheader_t;

void driver_init(void) {
  /* Serial and serial support pin setup */
  pinMode(DRIVER_ENABLE, OUTPUT);
  pinMode(RECEIVER_nENABLE, OUTPUT);
  driver_disable();
}

inline void driver_enable(void) {
  digitalWrite(DRIVER_ENABLE, HIGH);
  digitalWrite(RECEIVER_nENABLE, LOW);
}

inline void driver_disable(void) {
  digitalWrite(DRIVER_ENABLE, LOW);
  digitalWrite(RECEIVER_nENABLE, HIGH);
}

void setup() {
  /* EIA495 line driver init */
  driver_init();

  Serial.begin(1000000);

  send_packet(PACKET_LOG, (uint8_t*)"Alive", 6);

  pinMode(SWITCH0, INPUT_PULLUP);
  pinMode(SWITCH1, INPUT_PULLUP);
  pinMode(SWITCH2, INPUT_PULLUP);
  pinMode(SWITCH3, INPUT_PULLUP);
  pinMode(SWITCH4, INPUT_PULLUP);
  pinMode(SWITCH5, INPUT_PULLUP);

  randomSeed(analogRead(A4));
  delay(random(100));
  start_millis = millis();

  for (uint8_t i = 0; i < sizeof(input_pins); i++) {
    bool cur_val = digitalRead(input_pins[i]);
    if (cur_val) {
      BIT_SET(last_bitmap, i);
    } else {
      BIT_CLEAR(last_bitmap, i);
    }
    input_last[i] = start_millis;
  }
}

void send_packet(packet_type_t type, uint8_t* data, uint8_t len) {
  uint8_t packet_len = len + sizeof(pheader_t) + 3;
  uint8_t buf[packet_len];
  buf[0] = type;
  buf[1] = DEVICE_ID;
  memcpy(buf + 2, data, len);
  uint16_t crc = calc_crc(buf, packet_len - 3);
  buf[packet_len - 3] = crc >> 8;
  buf[packet_len - 2] = crc & 0xff;
  buf[packet_len - 1] = 0xC0;
  eia485_send(buf, packet_len);
}

void loop() {
  long loop_millis = millis();
  if (loop_millis >= last_millis + KEEPALIVE_INTERVAL_MSEC || last_millis > loop_millis) {
    /* Send keepalive at intervals */
    send_packet(PACKET_KEEPALIVE, &last_bitmap, 1);
    last_millis = loop_millis;
  }

  for (uint8_t i = 0; i < sizeof(input_pins); i++) {
    /* Button i pressed */
    bool cur_val = digitalRead(input_pins[i]);
    bool last_val = last_bitmap & (1 << i);
    if (cur_val != last_val) {                               // Only register changes
      if (loop_millis >= input_last[i] + DEBOUNCE_MILLIS) {  // Ignore pin flagging
        if (cur_val) {
          BIT_SET(last_bitmap, i);
        } else {
          BIT_CLEAR(last_bitmap, i);
        }
        input_last[i] = loop_millis;
        uint8_t buf[2];
        buf[0] = i;
        buf[1] = cur_val;
        send_packet(PACKET_INPUT, buf, 2);
        //send_packet(PACKET_LOG, (uint8_t *)"Press", strlen("Press")+1);
      }
    }
  }
}

void eia485_send(uint8_t *packet, size_t len) {
  driver_enable();
  Serial.write(packet, len);
  Serial.flush();
  driver_disable();
}

uint16_t calc_crc(uint8_t *msg, int n)
{
  uint16_t x = 0;

  while (n--) {
    x = crc_xmodem_update(x, (uint16_t) * msg++);
  }

  return (x);
}

uint16_t crc_xmodem_update (uint16_t crc, uint8_t data)
{
  int i;

  crc = crc ^ ((uint16_t)data << 8);
  for (i = 0; i < 8; i++) {
    if (crc & 0x8000)
      crc = (crc << 1) ^ 0x1021; //(polynomial = 0x1021)
    else
      crc <<= 1;
  }
  return crc;
}