#include "IO_ADI.h"

void initIO(void)
{
  // GPIO Setup
  pinMode(SPAREOUTPIN, OUTPUT); // Spare output
  digitalWrite(SPAREOUTPIN, HIGH);

  pinMode(33, OUTPUT); // Heartbeat LED

  pinMode(25, INPUT); // Spare input
  pinMode(35, INPUT); // Obstruction

  memset(io_array, 0, sizeof(io_array));

  pinMode(BATTERYINPUTPIN, INPUT);
}

void serviceIO(void)
{
  uint8_t io8;
  uint16_t io16;
  uint16_t mask;
  uint16_t i;

  // INPUTS
  io8 = readMCP23S08(IOEXP_CS, 9);

  if (bitRead(io8, 0) == 0)
    io_array[RET_LIMIT]++;
  else
    io_array[RET_LIMIT] = 0;
  if (bitRead(io8, 1) == 0)
    io_array[EXT_LIMIT]++;
  else
    io_array[EXT_LIMIT] = 0;
  if (bitRead(io8, 2) == 0)
    io_array[OPEN_LIMIT]++;
  else
    io_array[OPEN_LIMIT] = 0;
  if (bitRead(io8, 3) == 0)
    io_array[ADDRBUTTON]++;
  else
    io_array[ADDRBUTTON] = 0;
  if (bitRead(io8, 6) == 0)
    io_array[RF_KEYPAD]++;
  else
    io_array[RF_KEYPAD] = 0;

  io_array[SPARE_IN] = digitalRead(25);
  // io_array[ADDRBUTTON] = digitalRead(ADDR_PB);

  io_array[BATT_V] = read_BatteryVoltage();

  // OUTPUTS
  io8 = 0;

  bitWrite(io8, 4, (io_array[COMPRESSOR] & 1));
  bitWrite(io8, 5, (io_array[EN_12V_SW] & 1));
  bitWrite(io8, 7, (io_array[POWER_RLY] & 1));

  setMCP23S08(IOEXP_CS, 9, io8);

  digitalWrite(SPAREOUTPIN, io_array[SPARE_OUT]);
}

#define A2DSIZE 15
uint16_t A2D_array[A2DSIZE + 1];
uint16_t A2D_index = 0;

uint16_t A2D_count = 0; // v527 fix low batt error on startup

uint16_t read_BatteryVoltage(void)
{
  uint16_t i, j;
  float f, b;
  uint32_t batteryInput;

  i = analogRead(BATTERYINPUTPIN);
  // Serial.println(i);
  A2D_array[A2D_index++] = i;

  // uase the A2D_count it is the size of the A2DSIZE
  if (A2D_count < A2DSIZE)
    A2D_count++;

  if (A2D_index >= A2DSIZE)
    A2D_index = 0;

  batteryInput = 0;
  for (i = 0; i < A2D_count; i++)
    batteryInput += A2D_array[i];

  // use the count if less than the A2DSize for the average
  batteryInput = batteryInput / (A2D_count < A2DSIZE ? A2D_count : A2DSIZE);
  batteryInput = ((batteryInput)*parm[18]) / 1000; // 1 point cal

  b = float(batteryInput) / 100;

  f = -0.0321 * b * b;

  f = f + (1.652 * b);

  f = f - 3.07;

  f = f * 100;
  batteryInput = f;
  // Serial.print("float: ");Serial.print(f);Serial.print(" int: ");Serial.println(batteryInput);

  return (batteryInput / 10);
}

/*
uint16_t read_BatteryVoltage(void)
{
  uint16_t i, j;
  float f, b;
  uint32_t batteryInput;

  i  = analogRead(BATTERYINPUTPIN);
  //Serial.println(i);
  A2D_array[A2D_index++] = i;
  if (A2D_index >= A2DSIZE) A2D_index = 0;

  batteryInput = 0;
  for (i = 0; i < A2DSIZE; i++) batteryInput += A2D_array[i];

  batteryInput = ((batteryInput / A2DSIZE) * parm[18]) / 1000; //1 point cal


  b = float(batteryInput) / 100;

  f = -0.0321 * b * b;

  f = f + (1.652 * b);

  f = f - 3.07;

  f = f * 100;
  batteryInput = f;
  //Serial.print("float: ");Serial.print(f);Serial.print(" int: ");Serial.println(batteryInput);

  return (batteryInput / 10);
}
*/