// Mindsensors PCF8574 Driver
// Created By Scott Briscoe
// Based off of code writen by Xander and from RobotC example files.

#define     PCF8574_ADDRESS         0x76  // Change to match your sensor
#define     ALL_PINS_OFF            0


// Function Prototypes
void PCF8574Initialize (const tSensors SensorPort);
void PCF8574Reset (const tSensors SensorPort);
void PCF8574Error (const tSensors SensorPort, byte WroteMessage, byte ReadMessage);
byte PCF8574Read(const tSensors SensorPort);
void PCF8574Write (const tSensors SensorPort, byte OutputMessage );
void PCF8574PinOn (const tSensors SensorPort, byte PinNumber );
void PCF8574PinOff (const tSensors SensorPort, byte PinNumber );
void PCF8574PinOnOff (const tSensors SensorPort, byte PinNumber );
int PCF8574PinRead (const tSensors SensorPort, byte PinNumber);



// Sets Up the PCF8574 on a port and resets the device
void PCF8574Initialize (const tSensors SensorPort)
{
  SensorType[SensorPort] = sensorI2CCustom;
  PCF8574Reset(SensorPort);
}


// Resets device, turns off all pins, pins 1-8  //
void PCF8574Reset (const tSensors SensorPort)
{
  PCF8574Write(SensorPort, ALL_PINS_OFF);
}


// Notifies user there was an error
// Sensor may not be pluged in
// May be a electric component that failed
// May be an electric short
void PCF8574Error (const tSensors SensorPort, byte WroteMessage, byte ReadMessage)
{
    eraseDisplay();
     nxtDisplayTextLine(1,"PCF8574 Error");
     nxtDisplayTextLine(2,"Port: S%d ", ((int)SensorPort) +1);
     nxtDisplayTextLine(3,"Adr: 0x%x", PCF8574_ADDRESS);
     nxtDisplayTextLine(4,"Wrote: %d", (int)WroteMessage );
     nxtDisplayTextLine(5,"Read: %d",  (int)ReadMessage );

     nxtDisplayTextLine(7,"Check Hardware");

    while (true) // Loop Forever, must restart application
    {
      PlaySound(soundException);
      wait1Msec(10000);
    }
}


// Reads what pins that are open or closed, and returns a binary number representing pins
byte PCF8574Read(const tSensors SensorPort)
{
  byte ReplyMessage = 0;
  byte I2CMessage[3];
   const int MessageSize       = 0;
   const int PCF8574Address    = 1;

  I2CMessage[MessageSize]       = 1;
   I2CMessage[PCF8574Address]    = PCF8574_ADDRESS;

   while (nI2CStatus[SensorPort] == STAT_COMM_PENDING)
  {
     wait1Msec(2);
  }
  sendI2CMsg(SensorPort, I2CMessage[0], 1);

  while (nI2CStatus[SensorPort] == STAT_COMM_PENDING)
  {
     wait1Msec(2);
  }
   readI2CReply(SensorPort, ReplyMessage, 1);
   return (ReplyMessage);

}


// Writes a binary number to the device to open and close the pins
void PCF8574Write (const tSensors SensorPort, byte OutputMessage)
{
  byte I2CMessage[3];
   const int MessageSize       = 0;
   const int PCF8574Address    = 1;
   const int BinaryPinMessage  = 2;
   byte HardwareAfter;

  I2CMessage[MessageSize]       = 2;
   I2CMessage[PCF8574Address]    = PCF8574_ADDRESS;
   I2CMessage[BinaryPinMessage]  = ~OutputMessage;

   while (nI2CStatus[SensorPort] == STAT_COMM_PENDING)
  {
      wait1Msec(2);
  }
   sendI2CMsg(SensorPort, I2CMessage[0] ,0);

   // Error Checking
   HardwareAfter = PCF8574Read(SensorPort);
   if (OutputMessage != ~HardwareAfter)
   {
     PCF8574Error(SensorPort, OutputMessage, HardwareAfter);
   }
}


// Turn on a pin, nothing will happen if already on
// Does not affecting already on or off pins
// Use number 1-8 for PinNumber
void PCF8574PinOn (const tSensors SensorPort, byte PinNumber )
{
  byte HardwareBefore;
  byte MessageToSend;

  PinNumber = PinNumber -1;
  HardwareBefore = PCF8574Read(SensorPort);
  MessageToSend = (~HardwareBefore) | (1 << PinNumber);

  PCF8574Write(SensorPort, MessageToSend);
}


// Turn off a pin, nothing will happen if already off
// Does not affecting already on or off pins
// Use number 1-8 for PinNumber
void PCF8574PinOff (const tSensors SensorPort, byte PinNumber )
{
  byte HardwareBefore;
  byte MessageToSend;

  PinNumber = PinNumber -1;
  HardwareBefore = PCF8574Read(SensorPort);
  MessageToSend = (~HardwareBefore) & (~1 << PinNumber);

  PCF8574Write(SensorPort, MessageToSend);
}


// Turn on or off a single pin, off if already on, on if already off
// Does not affecting already on or off pins
// Use number 1-8 for PinNumber
void PCF8574PinOnOff (const tSensors SensorPort, byte PinNumber )
{
  byte HardwareBefore;
  byte MessageToSend;

  PinNumber = PinNumber -1;
  HardwareBefore = PCF8574Read(SensorPort);
  MessageToSend = (~HardwareBefore) ^ (1 << PinNumber);

  PCF8574Write(SensorPort, MessageToSend);
}


// Reads a single pin and return 1 if on, 0 if off
// Use number 1-8 for PinNumber
int PCF8574PinRead (const tSensors SensorPort, byte PinNumber)
{
  byte ReadHardware;
  byte SinglePin;

  PinNumber = PinNumber -1;
  ReadHardware = PCF8574Read(SensorPort);
  SinglePin = (~ReadHardware) & (1 << PinNumber);
  SinglePin = (SinglePin >> PinNumber);
  SinglePin = abs(SinglePin);
  return(SinglePin);
}