#include "coils.h"

void mbToCoils(ModbusSerial& mb);
void coilsToShiftOutRegisters(size_t i);
void outputShiftOutRegisters();

Coils coils;
ShiftOutRegistersUnion shiftOutRegisters;
size_t digitIndex;

void setupCoils(ModbusSerial& mb) {
  pinMode(DATA_PIN_595, OUTPUT);
  pinMode(OE_PIN_595, OUTPUT);
  pinMode(LATCH_PIN_595, OUTPUT);
  pinMode(CLOCK_PIN_595, OUTPUT);

  digitalWrite(OE_PIN_595, LOW);

  for (size_t i = 0; i < COIL_COUNT; i++)
    mb.addCoil (i);
}

void coilsTask(ModbusSerial& mb) {
  mbToCoils(mb);
  coilsToShiftOutRegisters(digitIndex);
  digitIndex = (digitIndex + 1) % 4;
  outputShiftOutRegisters();
}

void mbToCoils(ModbusSerial& mb) {
  coils.relays.J1 = mb.coil(0);
  coils.relays.J2 = mb.coil(1);
  coils.relays.J3 = mb.coil(2);
  coils.relays.J4 = mb.coil(3);
  coils.relays.J5 = mb.coil(4);
  coils.relays.J6 = mb.coil(5);
  coils.relays.J7 = mb.coil(6);
  coils.relays.J8 = mb.coil(7);

  coils.digit0.A = mb.coil(8);
  coils.digit0.B = mb.coil(9);
  coils.digit0.C = mb.coil(10);
  coils.digit0.D = mb.coil(11);
  coils.digit0.E = mb.coil(12);
  coils.digit0.F = mb.coil(13);
  coils.digit0.G = mb.coil(14);
  coils.digit0.H = mb.coil(15);

  coils.digit1.A = mb.coil(16);
  coils.digit1.B = mb.coil(17);
  coils.digit1.C = mb.coil(18);
  coils.digit1.D = mb.coil(19);
  coils.digit1.E = mb.coil(20);
  coils.digit1.F = mb.coil(21);
  coils.digit1.G = mb.coil(22);
  coils.digit1.H = mb.coil(23);

  coils.digit2.A = mb.coil(24);
  coils.digit2.B = mb.coil(25);
  coils.digit2.C = mb.coil(26);
  coils.digit2.D = mb.coil(27);
  coils.digit2.E = mb.coil(28);
  coils.digit2.F = mb.coil(29);
  coils.digit2.G = mb.coil(30);
  coils.digit2.H = mb.coil(31);

  coils.digit3.A = mb.coil(32);
  coils.digit3.B = mb.coil(33);
  coils.digit3.C = mb.coil(34);
  coils.digit3.D = mb.coil(35);
  coils.digit3.E = mb.coil(36);
  coils.digit3.F = mb.coil(37);
  coils.digit3.G = mb.coil(38);
  coils.digit3.H = mb.coil(39);
}

void coilsToShiftOutRegisters(size_t i) {
  switch(i) {
    case 0:
      shiftOutRegisters.shiftOutRegisters.u6 = coils.digit0;
      shiftOutRegisters.shiftOutRegisters.u8.G1 = 0;
      shiftOutRegisters.shiftOutRegisters.u8.G2 = 1;
      shiftOutRegisters.shiftOutRegisters.u8.G3 = 1;
      shiftOutRegisters.shiftOutRegisters.u8.G4 = 1;
      shiftOutRegisters.shiftOutRegisters.u5 = coils.relays;
      break;
    case 1:
      shiftOutRegisters.shiftOutRegisters.u6 = coils.digit1;
      shiftOutRegisters.shiftOutRegisters.u8.G1 = 1;
      shiftOutRegisters.shiftOutRegisters.u8.G2 = 0;
      shiftOutRegisters.shiftOutRegisters.u8.G3 = 1;
      shiftOutRegisters.shiftOutRegisters.u8.G4 = 1;
      shiftOutRegisters.shiftOutRegisters.u5 = coils.relays;
      break;
    case 2:
      shiftOutRegisters.shiftOutRegisters.u6 = coils.digit2;
      shiftOutRegisters.shiftOutRegisters.u8.G1 = 1;
      shiftOutRegisters.shiftOutRegisters.u8.G2 = 1;
      shiftOutRegisters.shiftOutRegisters.u8.G3 = 0;
      shiftOutRegisters.shiftOutRegisters.u8.G4 = 1;
      shiftOutRegisters.shiftOutRegisters.u5 = coils.relays;
      break;
    default:
      shiftOutRegisters.shiftOutRegisters.u6 = coils.digit3;
      shiftOutRegisters.shiftOutRegisters.u8.G1 = 1;
      shiftOutRegisters.shiftOutRegisters.u8.G2 = 1;
      shiftOutRegisters.shiftOutRegisters.u8.G3 = 1;
      shiftOutRegisters.shiftOutRegisters.u8.G4 = 0;
      shiftOutRegisters.shiftOutRegisters.u5 = coils.relays;
      break;
  }
}

void outputShiftOutRegisters() {
  digitalWrite(LATCH_PIN_595, LOW);
  shiftOut(DATA_PIN_595, CLOCK_PIN_595, MSBFIRST, shiftOutRegisters.bytes[2]);
  shiftOut(DATA_PIN_595, CLOCK_PIN_595, MSBFIRST, shiftOutRegisters.bytes[1]);
  shiftOut(DATA_PIN_595, CLOCK_PIN_595, MSBFIRST, shiftOutRegisters.bytes[0]);
  digitalWrite(LATCH_PIN_595, HIGH);
}