#include <Adafruit_PCF8575.h>
#include <Adafruit_PWMServoDriver.h>
/* Example for 16 input buttons that are connected from the GPIO expander pins to ground.
 * Note the buttons must be connected with the other side of the switch to GROUND. There is
 * a built in pull-up 'resistor' on each input, but no pull-down resistor capability.
 */

#define SERVOMIN  150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  600 // This is the 'maximum' pulse length count (out of 4096)
#define USMIN  600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
#define USMAX  2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates

Adafruit_PCF8575 pcf20;

int PINNO = 14;
bool pins[16];
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);



volatile bool trigered;

void buttonInterrupt() {
  trigered = true;
  // Serial.println("interupted");
  // Serial.flush();
}

void setup() {
  while (!Serial) { delay(10); }
  Serial.begin(115200);
  Serial.println("Adafruit PCF8575 button read test");
  for (uint8_t p = 0; p < 16; p++) {
    pins[p] = false;
  }
  if (!pcf20.begin(0x20, &Wire)) {
    Serial.println("Couldn't find PCF8575");
    while (1)
      ;
  }

  for (uint8_t p = 0; p < 16; p++) {
    if (p == 14) {
      pcf20.pinMode(p, OUTPUT);
    } else {
      pcf20.pinMode(p, INPUT_PULLUP);
    }
  }
  trigered = false;
  pinMode(3, INPUT_PULLUP);
  attachInterrupt(0, buttonInterrupt, CHANGE);
  delay(1000);
  Serial.println("setting pwm");
  pwm.setOscillatorFrequency(27000000);
  delay(1000);
  //pwm.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates
}

void loop() {
  if (!trigered)
    return;
  Serial.println("trigered");
  trigered = false;
  for (uint8_t p = 0; p < 16; p++) {
    if (p == 14)
      continue;
    if (!pcf20.digitalRead(p)) {
      if (!pins[p]) {
        Serial.print("Button on GPIO 20 #");
        Serial.print(p);
        Serial.println(" pressed!");
        pins[p] = true;
        pcf20.digitalWrite(14, true);
        if (p==0){
          pwm.setPWM(1,4096,0);
        }
      }
    } else if (pins[p]) {
      Serial.print("Button on GPIO 20 #");
      Serial.print(p);
      Serial.println(" released!");
      pins[p] = false;
      pcf20.digitalWrite(14, false);
      if (p==0){
          pwm.setPWM(1,0,4096);
        }
    }
  }
  delay(10);  // a short debounce delay
}