#define DEVICE_ID 0x00
#define KEEPALIVE_INTERVAL_MSEC 5000UL

#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 OUT0 3
#define OUT1 5 
#define OUT2 6
#define OUT3 9
#define OUT4 11
#define OUT5 10

static uint8_t pattern_buf[6] = {0,0,0,0,0};
static const uint8_t target_pattern[6] = {0, 1, 2, 3, 4, 5};
static const uint8_t input_pins[] = {SWITCH0, SWITCH1 ,SWITCH2, SWITCH3, SWITCH4, SWITCH5};
static const uint8_t output_pins[] = {OUT0, OUT1, OUT2, OUT3, OUT4, OUT5};
static int8_t last_input = -1, input_count = 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 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;
}

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 driver_init(void) {
  /* Serial and serial support pin setup */
  driver_disable();
  pinMode(DRIVER_ENABLE, OUTPUT);
  pinMode(RECEIVER_nENABLE, OUTPUT);
}

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() {
  pinMode(SWITCH0, INPUT_PULLUP);
  pinMode(SWITCH1, INPUT_PULLUP);
  pinMode(SWITCH2, INPUT_PULLUP);
  pinMode(SWITCH3, INPUT_PULLUP);
  pinMode(SWITCH4, INPUT_PULLUP);
  pinMode(SWITCH5, INPUT_PULLUP);

  pinMode(OUT0, OUTPUT);
  pinMode(OUT1, OUTPUT);
  pinMode(OUT2, OUTPUT);
  pinMode(OUT3, OUTPUT);
  pinMode(OUT4, OUTPUT);
  pinMode(OUT5, OUTPUT);
  set_all(false);


  /* Serial and serial support pin setup */
  driver_init();
  Serial.begin(1000000);

  set_all(true);
  delay(1000);
  set_all(false);
  randomSeed(analogRead(A4));
  delay(random(500));
  send_packet(PACKET_LOG, (uint8_t*) "Alive", 6);
}

void loop() {
    if (!(millis() % KEEPALIVE_INTERVAL_MSEC)) {
      /* Send keepalive every */
      send_packet(PACKET_KEEPALIVE, NULL, 0);
      delay(2);
    }
    for (uint8_t i = 0; i < sizeof(input_pins); i++) {
      /* Button i pressed */
      if (digitalRead(input_pins[i]) == LOW) {
        if (last_input == i) {
          /* Primitive debounce */
          continue;
        }
        /* Blink to acknowledge entry */
        digitalWrite(output_pins[i], !digitalRead(output_pins[i]));
        delay(100);
        digitalWrite(output_pins[i], !digitalRead(output_pins[i]));
        
        last_input = i;
        pattern_buf[input_count] = i;
        input_count += 1;
        
        /* Illumated buttons in correct positions */
        if (target_pattern[input_count-1] == i) {
          digitalWrite(output_pins[i], HIGH);
        }
        
        /* After six entries, see if the correct code has been entered */
        if (input_count == 6) {
          if (!memcmp(pattern_buf, target_pattern, sizeof(target_pattern))) {
            win();
          } else {
            send_packet(PACKET_FAILED, NULL, 0);
            delay(1000);
            nope();
            set_all(false);
            input_count = 0;
            last_input = -1;
          }
        }
        uint8_t buf[2];
        buf[0] = i;
        buf[1] = LOW;
        send_packet(PACKET_INPUT, buf, 2);
      }
    }
    delay(1);
}

void set_all(bool val) {
  for (uint8_t i = 0; i < 6; i++) {
    digitalWrite(output_pins[i], val);
  }
}

void win(void) {
  while(true) {
    set_all(true);
    delay(1000);
    set_all(false);
    delay(1000);
    send_packet(PACKET_SOLVED, NULL, 0);
  }
}

void nope(void) {
  for (uint8_t i = 0; i < 3; i++) {
    digitalWrite(OUT0, HIGH);
    digitalWrite(OUT2, HIGH);
    digitalWrite(OUT4, HIGH);
    digitalWrite(OUT1, LOW);
    digitalWrite(OUT3, LOW);
    digitalWrite(OUT5, LOW);
    delay(500);
    digitalWrite(OUT0, LOW);
    digitalWrite(OUT2, LOW);
    digitalWrite(OUT4, LOW);
    digitalWrite(OUT1, HIGH);
    digitalWrite(OUT3, HIGH);
    digitalWrite(OUT5, HIGH);
    delay(500);
  }
}