#include "discrete_inputs.h"

ShiftInRegisterUnion shiftInRegister;
Keys keys;
DiscreteInputs discreteInputs;

void setupDiscreteInputs(ModbusSerial& mb) {
  pinMode(KEY_1_PIN, INPUT_PULLUP);
  pinMode(KEY_2_PIN, INPUT_PULLUP);
  pinMode(KEY_3_PIN, INPUT_PULLUP);
  pinMode(KEY_4_PIN, INPUT_PULLUP);

  pinMode(LOAD_PIN_165, OUTPUT);
  pinMode(CLOCK_PIN_165, OUTPUT);
  pinMode(DATA_PIN_165, INPUT);

  for (size_t i = 0; i < DISCRETE_INPUT_COUNT; i++)
    mb.addIsts(i);
}

void discreteInputsTask(ModbusSerial& mb) {
  keys.KEY_1 = !digitalRead(KEY_1_PIN);
  keys.KEY_2 = !digitalRead(KEY_2_PIN);
  keys.KEY_3 = !digitalRead(KEY_3_PIN);
  keys.KEY_4 = !digitalRead(KEY_4_PIN);

  digitalWrite(CLOCK_PIN_165, HIGH);
  digitalWrite(LOAD_PIN_165, LOW);
  digitalWrite(LOAD_PIN_165, HIGH);
  shiftInRegister.byte = shiftIn(DATA_PIN_165, CLOCK_PIN_165, MSBFIRST) ^ 0XFF;

  discreteInputs.digitalInputs = shiftInRegister.shiftInRegister;
  discreteInputs.keys = keys;

  mb.setIsts(0, discreteInputs.digitalInputs.INPUT1);
  mb.setIsts(1, discreteInputs.digitalInputs.INPUT2);
  mb.setIsts(2, discreteInputs.digitalInputs.INPUT3);
  mb.setIsts(3, discreteInputs.digitalInputs.INPUT4);
  mb.setIsts(4, discreteInputs.digitalInputs.INPUT5);
  mb.setIsts(5, discreteInputs.digitalInputs.INPUT6);
  mb.setIsts(6, discreteInputs.digitalInputs.INPUT7);
  mb.setIsts(7, discreteInputs.digitalInputs.INPUT8);

  mb.setIsts(8, discreteInputs.keys.KEY_1);
  mb.setIsts(9, discreteInputs.keys.KEY_2);
  mb.setIsts(10, discreteInputs.keys.KEY_3);
  mb.setIsts(11, discreteInputs.keys.KEY_4);
}