// Interfacing MCP23017 E/SP with arrays in Arduino IDE
// https://forum.arduino.cc/t/interfacing-mcp23017-e-sp-with-arrays-in-arduino-ide/1309545


#include <LiquidCrystal.h> //Library for LCD screen
#include <MCP23017.h> //Library for expansion chips
// #include <Wire.h>

MCP23017 mcp[] = {0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27};
#define FISRT_MCP_PIN 101
#define  OFFSET 1  // Count from 0 but display from 1 up

enum {PASS, FAIL_NOTCONNECTED, FAIL_WRONGCONNECTED, FAIL_SHORTENED };

//Pins for 715 test

const byte Cable715Begin[] = { 207, 208, 209, 210, 211, 176};

const byte Cable715End[]  =  { 174, 172, 173, 175, 176, 177};

const byte NUM715CABLES = sizeof(Cable715Begin);


void setup() { //setup code
  Wire.begin();
  Serial.begin (115200); //initialising serial monitor used for testing purposes
  Serial.println("beginning program");

  uint8_t MCP23017_count = sizeof(mcp) / sizeof(* mcp);
  // Serial.print(MCP23017_count);
  // Serial.println(" MCP23017 declared");
  for (size_t mcpIndex = 0; mcpIndex < MCP23017_count; mcpIndex++)
  {
    mcp[mcpIndex].init();
  }

  for (byte pinIndex = 0; pinIndex < NUM715CABLES; pinIndex++) // test each pin
  {
    Serial.print("Wire ");
    Serial.print(pinIndex + OFFSET);
    Serial.print(" is between Chip ");
    Serial.print((Cable715Begin[pinIndex] - FISRT_MCP_PIN) / 16 + OFFSET);
    Serial.print(" Pin ");
    Serial.print((Cable715Begin[pinIndex] - FISRT_MCP_PIN) % 16 + OFFSET);
    Serial.print(" & Chip ");
    Serial.print((Cable715End[pinIndex] - FISRT_MCP_PIN) / 16 + OFFSET);
    Serial.print(" Pin ");
    Serial.println((Cable715End[pinIndex] - FISRT_MCP_PIN) % 16 + OFFSET);
  }

  Serial.print("Setup Done");
  while(1);
  Do715Test();
}

void myPinMode(uint8_t pinNumber, uint8_t pinModeVal)
{
  if (pinNumber < FISRT_MCP_PIN)
  {
    pinMode(pinNumber, pinModeVal);
  }
  else
  {
    mcp[(pinNumber - FISRT_MCP_PIN) / 16].pinMode((pinNumber - FISRT_MCP_PIN) % 16, pinModeVal, false);
  }
}

void myDigitalWrite(uint8_t pinNumber, uint8_t pinState)
{
  if (pinNumber < FISRT_MCP_PIN)
  {
    digitalWrite(pinNumber, pinState);
  }
  else
  {
    mcp[(pinNumber - FISRT_MCP_PIN) / 16].digitalWrite((pinNumber - FISRT_MCP_PIN) % 16, pinState);
  }
}

int8_t myDigitalRead(uint8_t pinNumber)
{
  if (pinNumber < FISRT_MCP_PIN)
  {
    return digitalRead(pinNumber);
  }
  else
  {
    return mcp[(pinNumber - FISRT_MCP_PIN) / 16].digitalRead((pinNumber - FISRT_MCP_PIN) % 16);
  }
}

void allPinsInputHigh715()
{ // set all pins to INPUT_PULLUP in a for-loop
  for (byte i = 0; i < NUM715CABLES; i++)
  {
    myPinMode(Cable715Begin[i], INPUT_PULLUP);
    myPinMode(Cable715End[i], INPUT_PULLUP);
  }
}



void Do715Test()
{
  byte result;



  Serial.println();
  Serial.println(" TEST 715 beginning");


  allPinsInputHigh715();



  for (byte i = 0; i < NUM715CABLES; i++) // test each pin
  {
    result = PASS; // initially there is no error found, assume PASS

    myPinMode(Cable715Begin[i], OUTPUT);
    myDigitalWrite(Cable715Begin[i], HIGH); // <- Don't forget to set the level too !!!

    // first test is for logic high continuity so OUTPUT/HIGH

    if (myDigitalRead(Cable715End[i]) != HIGH)
    {
      bitSet(result, FAIL_NOTCONNECTED);
      Serial.println(" break on high");
      allPinsInputHigh715(); //restting all fault pins high
    }
    // then check for logic low continuity so OUTPUT/LOW
    myDigitalWrite(Cable715Begin[i], LOW);
    delay(1000);
    if (myDigitalRead(Cable715End[i]) != LOW)
    {
      bitSet(result, FAIL_NOTCONNECTED);
      Serial.println(" break on low");
      allPinsInputHigh715(); //restting all fault pins high
    }

    // next test: check for wrong connections to other pins
    for (byte j = 0; j < NUM715CABLES; j++)
    {
      if (j != i && myDigitalRead(Cable715End[j]) == LOW)
      {
        bitSet(result, FAIL_WRONGCONNECTED);
        allPinsInputHigh715(); //restting all fault pins high
      }
    }
    // final test for short circuit against other pins
    for (byte j = 0; j < NUM715CABLES; j++)
    {
      if (j != i && myDigitalRead(Cable715Begin[j]) == LOW)
      {
        bitSet(result, FAIL_SHORTENED);
        allPinsInputHigh715(); //restting all fault pins high
      }
    }
    Serial.print("Line ");
    Serial.print(i + 1);

    myPinMode(Cable715Begin[i], INPUT_PULLUP); //resetting current testing pin back to high


    if (result == PASS) Serial.print(" PASS");
    else Serial.print(" FAIL");


    if (bitRead(result, FAIL_NOTCONNECTED)) Serial.print(" BREAK");
    if (bitRead(result, FAIL_WRONGCONNECTED)) Serial.print(" WRONG");
    if (bitRead(result, FAIL_SHORTENED)) Serial.print(" SHORT");
    Serial.println();
  }
  Serial.println("Test finished.");
  Serial.println();
}

void loop() {
  delay(20);
}