//wokwi.com/projects/396714529824912385
// za Boha nefunguje u2c scan i kdyz je zkopirovanej z  //wokwi.com/projects/462494525938281473 a tam funguje
//    už to umí ISR :-)
//    vektor vektorů ISR na PCF857 device(7)  -  BACHA na i2c adr LCD displej
//    vymyslet čtení z device(7) a volat jen odpovídající mirror, mirror device(7) funguje
//

// Using both I2C Bus Interfaces of ESP32
// ESP32 I2C Communication Set Pins, Multiple Devices Interfaces and Change Pins
// https://microcontrollerslab.com/esp32-i2c-communication-tutorial-arduino-ide/

#include <Wire.h>                   // I2C included in ARDU ?
#include <Adafruit_PWMServoDriver.h>  // PWMservo library
#include <I2cDiscreteIoExpander.h>    // PCF85xx library
#include <LiquidCrystal_I2C.h>        // LCD I2C library

#include <Adafruit_PCF8574.h>           // library for PCF8574-PCF8575
//#include <PCF8575.h>                    // library for PCF8575

#define PCA9685_ADDRESS_1  0x40         // I2C PCA9685
#define PCA9685_ADDRESS_2  0x41         // I2C PCA9685
#define PCA9685_ADDRESS_3  0x42         // I2C PCA9685
#define PCA9685_ADDRESS_4  0x43         // I2C PCA9685
#define TCAADDR 0x70                  // TCA9548A I2C Multiplexer

#define Add0 35     // PCF chip1 INToutput read     //   puvodne bylo na 32-to potrebuju pro i2c_2

#define LEDPIN 25
#define DEBOUNCE_DELAY 100      // puvodni 200

volatile bool interrupt = false;    // true on interrupt
volatile bool ledState = false;
volatile uint32_t lastInterruptTime = 0;

// I2C LCD display addresses    - musí být nastavena podle .json 
const int lcdAddress1 = 0x30;   // Replace with the actual I2C address of LCD 1

LiquidCrystal_I2C lcd(lcdAddress1, 16, 2);   // LCD on Address 0x27 puvodní

Adafruit_PCF8574 pcf;
//PCF8575 pcf;

Adafruit_PWMServoDriver pwm1 = Adafruit_PWMServoDriver(PCA9685_ADDRESS_1);
Adafruit_PWMServoDriver pwm2 = Adafruit_PWMServoDriver(PCA9685_ADDRESS_2);
Adafruit_PWMServoDriver pwm3 = Adafruit_PWMServoDriver(PCA9685_ADDRESS_3);
Adafruit_PWMServoDriver pwm4 = Adafruit_PWMServoDriver(PCA9685_ADDRESS_4);

#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 SERVO_FREQ 50  // Analog servos run at ~50 Hz updates

#define SDA_2 33
#define SCL_2 32


// Adjust these angles according to Servo360_PCA_AngleFInd code that I have provided
int leftAngle = 0;     // angel for motor to move left (If the motor move in right you can change the name to rightAngle)
int rightAngle = 180;  // angel for motor to move right (If the motor move in left you can change the name to leftAngle)
int stopAngle = 90;    // Angle when motor stops

// our servo # counter
uint8_t servonum = 0;
byte totalServos = 16;  // Define how many servos we are using here

// Define the time (in milliseconds 1 second = 1000 millisecond) for each servo to move in right direction
int rightMoveTime[13] = { 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100 };  // Wait for this millisecond when motor move right
int stopWaitTime[13] =  { 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100 };   // Wait for this millisecond after motor has reached it's point
int leftMoveTime[13] =  { 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100 };   // Wait for this millisecond when motor move left

// create I2cDiscreteIoExpander objects
I2cDiscreteIoExpander device[8] = {0,1,2,3,4,5,6,7};  //  [8=total number]

// initial display cursor :
int cursorX = 0;
int cursorY = 0;

int p = 0;
int m = 0;
// Serial. tady funguje ale LCD displ havaruje
//void IRAM_ATTR doISR(){   // Serial. tady funguje ale LCD displ havaruje
void ARDUINO_ISR_ATTR doISR() {
  uint32_t currentTime = millis();
  if (currentTime - lastInterruptTime > DEBOUNCE_DELAY) {     // <<<---- tady je to kouzlo
    interrupt = true;
    Serial.print("ISR LED: ");
    if (ledState) {
      Serial.print("ON");
    } else {
      Serial.print("OFF");
    }
    Serial.println();
    lastInterruptTime = currentTime;
    //  if (higherPriorityTaskWoken) {
    ///      portYIELD_FROM_ISR();    // má význam pro tasky a priority viz RND
    ledState = !ledState;   // jen střídavě přepíná
  }
}
void i2cScan();
void i2c_1Scan();

void setup() {
pinMode(Add0, INPUT_PULLUP);
pinMode(34, INPUT_PULLUP);

  // initialize i2c interface
  Wire.begin();  // bez toho to samozrejme blblo !!!
  Wire1.begin(SDA_2, SCL_2);

  // initialize serial interface
  Serial.begin(115200);

//  tcaselect(0);   // Select the PCF channel
  Serial.println("setup i2c test :");
  i2cScan();
  i2c_1Scan();

  Serial.println("setup i2c test done");
  Serial.println("");
  delay(1000);

  lcd.init();
  lcd.backlight();

      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("PCA9685 test");
      lcd.setCursor(0, 1);
      lcd.print("PCF8575 test");

  // PCF init preset
  // bez nasled disable to nehraje

  for(uint8_t i = 0; i<8; i++){
  device[i].disableBitwiseInversion();
  }
  /*
  device[0].disableBitwiseInversion();
  device[1].disableBitwiseInversion();
  device[2].disableBitwiseInversion();
  device[3].disableBitwiseInversion();
  device[4].disableBitwiseInversion();
  device[5].disableBitwiseInversion();
  device[6].disableBitwiseInversion();
  device[7].disableBitwiseInversion();
  */
/*****************************************************************************************************/
//  doplneno pro wire1

  Serial.println("Adafruit i2c_1 bus PCF8575 button read test");

  if (!pcf.begin(0x20, &Wire1)) {
    Serial.println("Couldn't find PCF8575 on i2c_1 bus");
    while (1);
  }
  for (uint8_t p=0; p<16; p++) {
    pcf.pinMode(p, INPUT_PULLUP);
  }

/******************************************************************************************************/
  pwm1.begin();  // Initialize PWM instance
  pwm1.setOscillatorFrequency(27000000);
  pwm1.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates

  pwm2.begin();  // Initialize PWM instance
  pwm2.setOscillatorFrequency(27000000);
  pwm2.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates

  pwm3.begin();  // Initialize PWM instance
  pwm3.setOscillatorFrequency(27000000);
  pwm3.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates

  pwm4.begin();  // Initialize PWM instance
  pwm4.setOscillatorFrequency(27000000);
  pwm4.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates

  // Stop all servos
  for (int servoNum = 0; servoNum < totalServos; servoNum++) {  // initially stop all servo motors
    pwm1.setPWM(servoNum, 0, getAnglePulse(stopAngle));
  }

  Serial.println("Clearing any int/mirroring inputs");
  for(uint8_t i = 0; i<8; i++){
  mirrorInputOntoOutputs(i);
  }
  /*
  mirrorInputOntoOutputs(0);
  mirrorInputOntoOutputs(1);
  mirrorInputOntoOutputs(2);
  mirrorInputOntoOutputs(3);
  mirrorInputOntoOutputs(4);
  mirrorInputOntoOutputs(5);
  mirrorInputOntoOutputs(6);
  mirrorInputOntoOutputs(7);
  */
  readInputs(4);

  pinMode(LEDPIN, OUTPUT);

  Serial.println("\n\nESP32 interrupt test multiple PCF");

//  pcf.digitalWrite(0,0);
//  pcf.digitalWrite(7,0);
  Wire1.beginTransmission(0x20);
  Wire1.write(0x55);
  Wire1.write(0x1E);
  Wire1.endTransmission();
  delay(50);
  Wire1.beginTransmission(0x20);
  Serial.println(Wire1.read());   // tady to zatim dava -1
  Serial.println(Wire1.read());   // tady to zatim dava -1
  Wire1.endTransmission();

  attachInterrupt(Add0, doISR, RISING);

}

void loop() {
  
  m = 0;
  p = 0;

  digitalWrite(LEDPIN, ledState);     // POZOR - dlouhá loop - dlouhá odezva - ošetřit jinak

//  for(;;){      // tady to dat do ISR, kdyz je to ve for() tak se cykli jen v nem 

if (digitalRead(Add0)== LOW){
//      Serial.println("===== mirror 1 =====");
      //delay(20);
      mirrorInputOntoOutputs(0);
//      Serial.println("===== mirror 2 =====");
      //delay(20);
      mirrorInputOntoOutputs(1);
//      Serial.println("===== mirror 3 =====");
      //delay(20);
      mirrorInputOntoOutputs(2);
//      Serial.println("===== mirror 4 =====");
      //delay(20);
      mirrorInputOntoOutputs(3);
//      Serial.println("===== mirror 5 =====");
      //delay(20);
    //  mirrorInputOntoOutputs(4);
      readInputs(4);
//      Serial.println("===== mirror 6 =====");
      //delay(20);
      mirrorInputOntoOutputs(5);

      Serial.println("ISR vectors on device(7)");
      //delay(20);
      mirrorInputOntoOutputs(7);
    }

//  }

//
//  while(digitalRead(34) == HIGH){
  if(digitalRead(34) == HIGH){
    if(m == 0){
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Servomotors run");
//      lcd.setCursor(0, 1);
//      lcd.print("runs]");
      m = 1;
    }
    // Run one servo at a time
// test pwm nr 1 only:
    for (int currentServo = 0; currentServo < totalServos; currentServo++) {
      pwm1.setPWM(currentServo, 0, getAnglePulse(rightAngle));  // First move servo to right side
      delay(rightMoveTime[currentServo]);                                    // move motor in right for this time defined in start
      pwm1.setPWM(currentServo, 0, getAnglePulse(stopAngle));   // Stop servo at this point
      delay(stopWaitTime[currentServo]);                                    // Keep motor stopped at this position defined in start
      pwm1.setPWM(currentServo, 0, getAnglePulse(leftAngle));   // Now move servo to left side back
      delay(leftMoveTime[currentServo]);                                     // move motor in left for this time defined in start
      pwm1.setPWM(currentServo, 0, getAnglePulse(stopAngle));   // Stop This servo
      delay(50);                                              // wait for 50 milliseconds before moving next servo
      }
// test pwm nr 2 only:
    for (int currentServo = 0; currentServo < totalServos; currentServo++) {
      pwm2.setPWM(currentServo, 0, getAnglePulse(rightAngle));  // First move servo to right side
      delay(rightMoveTime[currentServo]);                                    // move motor in right for this time defined in start
      pwm2.setPWM(currentServo, 0, getAnglePulse(stopAngle));   // Stop servo at this point
      delay(stopWaitTime[currentServo]);                                    // Keep motor stopped at this position defined in start
      pwm2.setPWM(currentServo, 0, getAnglePulse(leftAngle));   // Now move servo to left side back
      delay(leftMoveTime[currentServo]);                                     // move motor in left for this time defined in start
      pwm2.setPWM(currentServo, 0, getAnglePulse(stopAngle));   // Stop This servo
      delay(50);                                              // wait for 50 milliseconds before moving next servo
    }
// test pwm nr 3 only:
    for (int currentServo = 0; currentServo < totalServos; currentServo++) {
      pwm3.setPWM(currentServo, 0, getAnglePulse(rightAngle));  // First move servo to right side
      delay(rightMoveTime[currentServo]);                                    // move motor in right for this time defined in start
      pwm3.setPWM(currentServo, 0, getAnglePulse(stopAngle));   // Stop servo at this point
      delay(stopWaitTime[currentServo]);                                    // Keep motor stopped at this position defined in start
      pwm3.setPWM(currentServo, 0, getAnglePulse(leftAngle));   // Now move servo to left side back
      delay(leftMoveTime[currentServo]);                                     // move motor in left for this time defined in start
      pwm3.setPWM(currentServo, 0, getAnglePulse(stopAngle));   // Stop This servo
      delay(50);                                              // wait for 50 milliseconds before moving next servo
  }
// test pwm nr 4 only:
    for (int currentServo = 0; currentServo < totalServos; currentServo++) {
      pwm4.setPWM(currentServo, 0, getAnglePulse(rightAngle));  // First move servo to right side
      delay(rightMoveTime[currentServo]);                                    // move motor in right for this time defined in start
      pwm4.setPWM(currentServo, 0, getAnglePulse(stopAngle));   // Stop servo at this point
      delay(stopWaitTime[currentServo]);                                    // Keep motor stopped at this position defined in start
      pwm4.setPWM(currentServo, 0, getAnglePulse(leftAngle));   // Now move servo to left side back
      delay(leftMoveTime[currentServo]);                                     // move motor in left for this time defined in start
      pwm4.setPWM(currentServo, 0, getAnglePulse(stopAngle));   // Stop This servo
      delay(50);                                              // wait for 50 milliseconds before moving next servo
    }
//
//  while (digitalRead(34) == LOW) {
  if (digitalRead(34) == LOW) {
    if(p == 0){
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Servos stopped");
//      lcd.setCursor(0, 1);
//      lcd.print("STOP");
      p = 1;
    }
  } 
  }
}
/******************************************************************************************/

int getAnglePulse(int inpAngle) {
  int retPulse = map(inpAngle, leftAngle, rightAngle, SERVOMIN, SERVOMAX);
  return retPulse;
}

uint16_t getChangedInputValue(uint8_t idx) {
//    Serial.println("Get Input");
    device[idx].digitalRead();
    return device[idx].getPorts();
}

void mirrorInputOntoOutputs(uint8_t idx) {

    uint16_t bothports = getChangedInputValue(idx);
    uint16_t inputs = lowByte(bothports);
    // Mirror the inputs to the outputs
    // leave the inputs, as inputs (i.e. writing high)
    uint16_t newValue = word(inputs, 0xff) ; // (inputs << 8) | 0xff;
/*    Serial.print("Inputs read as 0x");
    Serial.print(inputs, HEX);
*/    //Serial.println(" being mirrored to out");
    device[idx].digitalWrite(newValue);
/*    Serial.print("Wrote 2 bytes: port0 -> 0x");
    Serial.print(lowByte(newValue), HEX);
    Serial.print(" port1 -> 0x");
    Serial.print(highByte(newValue), HEX);
    Serial.print(" to device ");
    Serial.println((int)idx);
*/  }

void readInputs(uint8_t idx) {
    uint16_t newValue = getChangedInputValue(idx);
    Serial.print("Inputs read as 0x");
    Serial.print(newValue, HEX);
    Serial.print(" on device ");
    Serial.println((int)idx);
//     if(newValue < 0xFFFE)
      if (lowByte(newValue) > 0xFD)  device[idx].digitalWrite (0xFFFE);
      else device[idx].digitalWrite (0xFFFF);
  
  }

void tcaselect(uint8_t i) {
  if (i > 7) return;
  Wire.beginTransmission(TCAADDR);
  Wire.write(1 << i);
  Wire.endTransmission();  
}

void i2cScan() {
  byte error, address;
  int nDevices;
  Serial.println("Scanning bus 0 ...");
  nDevices = 0;
  for(address = 1; address < 127; address++ ) {
    Wire.beginTransmission(address);
    error = Wire.endTransmission();
    if (error == 0) {
      Serial.print("I2C device found at address 0x");
      if (address<16) {
        Serial.print("0");
      }
      Serial.println(address,HEX);
      nDevices++;
    }
    else if (error==4) {
      Serial.print("Unknow error at address 0x");
      if (address<16) {
        Serial.print("0");
      }
      Serial.println(address,HEX);
    }    
  }
  if (nDevices == 0) {
    Serial.println("No I2C devices found\n");
  }
  else {
    Serial.println("done\n");
  }
  delay(1000);          
}

void i2c_1Scan() {
  byte error, address;
  int nDevices;
  Serial.println("Scanning bus 1 ...");
  nDevices = 0;
  for(address = 1; address < 127; address++ ) {
    Wire1.beginTransmission(address);
    error = Wire1.endTransmission();
    if (error == 0) {
      Serial.print("I2C device found at address 0x");
      if (address<16) {
        Serial.print("0");
      }
      Serial.println(address,HEX);
      nDevices++;
    }
    else if (error==4) {
      Serial.print("Unknow error at address 0x");
      if (address<16) {
        Serial.print("0");
      }
      Serial.println(address,HEX);
    }    
  }
  if (nDevices == 0) {
    Serial.println("No I2C devices found\n");
  }
  else {
    Serial.println("done\n");
  }
  delay(1000);          
}