#include <Wire.h>
#include <TimerOne.h>



#define Mcp_Reg_IODIRA			0x00  	// IO direction  (0 = output, 1 = input (Default))
#define Mcp_Reg_IODIRB			0x01
#define Mcp_Reg_IOPOLA			0x02  	// IO polarity   (0 = normal, 1 = inverse)
#define Mcp_Reg_IOPOLB   		0x03
#define Mcp_Reg_GPINTENA 		0x04  	// Interrupt on change (0 = disable, 1 = enable)
#define Mcp_Reg_GPINTENB 		0x05
#define Mcp_Reg_DEFVALA  		0x06  	// Default comparison for interrupt on change (interrupts on opposite)
#define Mcp_Reg_DEFVALB  		0x07
#define Mcp_Reg_INTCONA  		0x08  	// Interrupt control (0 = interrupt on change from previous, 1 = interrupt on change from DEFVAL)
#define Mcp_Reg_INTCONB  		0x09
#define Mcp_Reg_IOCON    		0x0A  	// IO Configuration: bank/mirror/seqop/disslw/haen/odr/intpol/notimp
//#define Mcp_Reg_IOCON 		0x0B 		// same as 0x0A
#define Mcp_Reg_GPPUA    		0x0C  	// Pull-up resistor (0 = disabled, 1 = enabled)
#define Mcp_Reg_GPPUB    		0x0D
#define Mcp_Reg_INFTFA   		0x0E  	// Interrupt flag (read only) : (0 = no interrupt, 1 = pin caused interrupt)
#define Mcp_Reg_INFTFB   		0x0F
#define Mcp_Reg_INTCAPA  		0x10  	// Interrupt capture (read only) : value of GPIO at time of last interrupt
#define Mcp_Reg_INTCAPB  		0x11
#define Mcp_Reg_GPIOA    		0x12  	// Port value. Write to change, read to obtain value
#define Mcp_Reg_GPIOB    		0x13
#define Mcp_Reg_OLLATA   		0x14   	// Output latch. Write to latch output.
#define Mcp_Reg_OLLATB   		0x15

#define mcp                             0x21

#define MCPResetPin			4		// hier hängt der MCP mit seinem Reset-Pin dran
#define MCPInterruptA	                0		// hier hängt der MCP mit seinem Interrupt-Pin 19 dran
#define MCPInterruptB    	        1		// hier hängt der MCP mit seinem Interrupt-Pin 19 dran
#define LEDOnBoard			13		// pin 13, LED aufm Arduino-Board

byte WireTransmitResult = 0;
volatile boolean SwitchPressed = false;
volatile boolean PolledTimer = false;
volatile boolean isInInit = false;
byte inputStateA = 0;
byte inputStateB = 0;
byte inputLastStateA = 0;
byte inputLastStateB = 0;
int lastRead = 0;

byte OutStateA = 0;
byte OutStateB = 0;

void setup()
{
  Serial.begin(9600);
  Serial.println("Start");
  Wire.begin();
  initiic();
  pinMode (LEDOnBoard, OUTPUT);
  digitalWrite (LEDOnBoard, LOW);
  // attachInterrupt(MCPInterruptA, ISRSwitchPressed, FALLING);
  // attachInterrupt(MCPInterruptB, ISRSwitchPressed, FALLING);
  Serial.println("Bereit");
  Timer1.initialize(50000); // 10 Hz
  //  Timer1.initialize(50000); // 20 Hz
  //  Timer1.initialize(25000); // 40 Hz
  Timer1.attachInterrupt( ISRTimer ); // attach the service routine here
}



void loop()
{
  // if (SwitchPressed || (millis() - lastRead >= 4000))
  if (SwitchPressed || PolledTimer)
  {
    digitalWrite (LEDOnBoard, HIGH);

    //    inputStateA = readiicbyte(mcp, Mcp_Reg_INTCAPA);
    //    inputStateB = readiicbyte(mcp, Mcp_Reg_INTCAPB);
    inputStateA = readiicbyte(mcp, Mcp_Reg_GPIOA);
    inputStateB = readiicbyte(mcp, Mcp_Reg_GPIOB);
    //    inputStateA = readiicbyte(mcp, Mcp_Reg_OLLATA);
    //    inputStateB = readiicbyte(mcp, Mcp_Reg_OLLATB);

    if (inputStateA != inputLastStateA || inputStateB != inputLastStateB)
    {
      for (byte i = 0; i <= 7; i++)
      {
        if (inputStateA & (1 << i))
        {
          OutStateA ^= (1 << i);
        }
        if (inputStateB & (1 << i))
        {
          OutStateB ^= (1 << i);
        }
      }

      Serial.print(OutStateA, BIN);
      Serial.print("  ");
      Serial.print(OutStateB, BIN);
      Serial.println("\n");

      inputLastStateA = inputStateA;
      inputLastStateB = inputStateB;

      digitalWrite (LEDOnBoard, LOW);

      lastRead = millis();
      SwitchPressed = false;
      PolledTimer = false;
    }
  }
}



void ISRSwitchPressed()
{
  if (!SwitchPressed && !isInInit)
  {
    SwitchPressed = true;
  }
}



void ISRTimer()
{
  if (!SwitchPressed && !isInInit && !PolledTimer)
  {
    PolledTimer = true;
  }
}


void writeiic(uint8_t adr, uint8_t port, byte value)
{
  Wire.beginTransmission( adr );
  Wire.write( port );
  Wire.write( value );
  WireTransmitResult = Wire.endTransmission();

  //  String t = "writeiic: ";
  //  t += " Adr: " + String(adr) + " Port: " + String(port) + " Value: " + String(value);
  //  Serial.println(t);
}



byte readiicbyte(uint8_t adr, uint8_t port)
{
  //Serial.println("IIC Lesen");
  byte returnword = 0x00;

  Wire.beginTransmission(adr);
  Wire.write(port);
  Wire.endTransmission();
  Wire.requestFrom((int)adr, 1);

  int c = 0;

  if (Wire.available())
  {
    returnword = Wire.read();
  }
  return returnword;
}



void initiic()
{
  Serial.println("Init Wire");
  isInInit = true;
  SwitchPressed = false;
  PolledTimer = false;

  pinMode(MCPResetPin, OUTPUT);
  digitalWrite(MCPResetPin, LOW);
  delay(5);
  digitalWrite(MCPResetPin, HIGH);

  writeiic(mcp, Mcp_Reg_IODIRA,	0x00);				// Alles auf Output setzen
  writeiic(mcp, Mcp_Reg_GPIOA,	0x00);				// Alle Pins auf 0 setzen
  writeiic(mcp, Mcp_Reg_IODIRB,	0x00);				// Alles auf Output setzen
  writeiic(mcp, Mcp_Reg_GPIOB,	0x00);				// alle Pins auf 0 setzen

  byte inputMaskA = 0xFF;
  byte inputMaskB = 0xFF;
  byte polA = 0xFF;
  byte polB = 0xFF;

  writeiic(mcp, Mcp_Reg_IODIRA, inputMaskA);		// Richtung (Ein-/Ausgang) einstellen
  writeiic(mcp, Mcp_Reg_IODIRB, inputMaskB);		// Richtung (Ein-/Ausgang) einstellen

  writeiic(mcp, Mcp_Reg_GPPUA, inputMaskA);		// Pull-Up Widerstände einrichten
  writeiic(mcp, Mcp_Reg_GPPUB, inputMaskB);		// Pull-Up Widerstände einrichten

  writeiic(mcp, Mcp_Reg_IOPOLA, polA);			// Polarität einstellen (für Eingänge 0 oder 1)
  writeiic(mcp, Mcp_Reg_IOPOLB, polB);			// Polarität einstellen (für Eingänge 0 oder 1)

  writeiic(mcp, Mcp_Reg_GPINTENA, inputMaskA);	        // Interrupt aktivieren
  writeiic(mcp, Mcp_Reg_GPINTENB, inputMaskB);	        // Interrupt aktivieren

  writeiic(mcp, Mcp_Reg_DEFVALA, inputMaskA);		// Interrupt Default-Wert festlegen
  writeiic(mcp, Mcp_Reg_DEFVALB, inputMaskB);		// Interrupt Default-Wert festlegen

  writeiic(mcp, Mcp_Reg_INTCONA, inputMaskA);		// Interrupt Vergleich einstellen, 0 = Änderung zum Vorgänger, 1 = Änderung zum DEFVAL-Wert
  writeiic(mcp, Mcp_Reg_INTCONB, inputMaskB);		// Interrupt Vergleich einstellen, 0 = Änderung zum Vorgänger, 1 = Änderung zum DEFVAL-Wert

  //  readiicbyte(mcp, Mcp_Reg_INTCAPA);			// Interrupts lesen und dadurch löschen
  //  readiicbyte(mcp, Mcp_Reg_INTCAPB);			// Interrupts lesen und dadurch löschen
  inputStateA = readiicbyte(mcp, Mcp_Reg_INTCAPA);
  inputStateB = readiicbyte(mcp, Mcp_Reg_INTCAPB);
  inputStateA = readiicbyte(mcp, Mcp_Reg_GPIOA);
  inputStateB = readiicbyte(mcp, Mcp_Reg_GPIOB);
  inputStateA = readiicbyte(mcp, Mcp_Reg_OLLATA);
  inputStateB = readiicbyte(mcp, Mcp_Reg_OLLATB);

  Serial.println("Init Fertig");
  isInInit = false;
}