//   tady funguje i2c scan v pohode
#include <Wire.h>
#include <I2cDiscreteIoExpander.h>    // PCF85xx library
#include <Adafruit_PWMServoDriver.h>  // PWMservo library
#include <FastLED.h>
#define NUM_LEDS 16
#define DATA_PIN 15
CRGB leds[NUM_LEDS];

#define Add0 32     // PCF chip1 INToutput read

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

/*
bool interrupt = false;    // true on interrupt
bool ledState = false;
uint32_t lastInterruptTime = 0;
uint16_t ISRVector;
*/
volatile bool interrupt = false;    // true on interrupt
volatile bool ledState = false;
volatile uint32_t lastInterruptTime = 0;
volatile uint16_t ISRVector;
uint16_t ISR_Xvector;

uint8_t ISRdev = 6;         // ISR vector device

/*volatile */ uint8_t error, address, reg, data;

uint8_t dev_addr = 0x40; //0x40 - 12C Address
uint8_t mode1 = 0;     //0x00 location for Mode1 register address
uint8_t mode2 = 1;     //0x01 location for Mode2 register address
uint16_t PRE_SCALE =30; 
uint8_t led0 = 6;      //0x06 location for start  LED0 registers
uint8_t rst = 1;       //0x01 reset device
uint8_t rval = 0;      //read register value

uint16_t led_on;
uint16_t led_off;
uint8_t ledN;

uint8_t led_stat[16];

/**********************************************************************/
#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

// 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

// 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 = 2;  // 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] = { 1000, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100 };  // Wait for this millisecond when motor move right
int stopWaitTime[13] =  { 1000, 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] =  { 1000, 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]


uint8_t PCF_addr[8] = {0x20,0x21,0x22,0x23,0x24,0x25,0x26,0x27};
uint8_t PCA_addr[8] = {0x40,0x41,0x42,0x43,0x44,0x45,0x46,0x47};

/********************************** ISR *********************************/

void IRAM_ATTR doISR(){   // Serial. tady funguje ale LCD displ havaruje
//void ARDUINO_ISR_ATTR doISR() {
  uint32_t currentTime = millis();
//  uint16_t ISRVector;

  if (currentTime - lastInterruptTime > DEBOUNCE_DELAY) {     // <<<---- tady je to kouzlo
/////    ISRVector = getChangedInputValue(ISRdev);
///   device[ISRdev].digitalRead();

//    Serial.print("ISRVector in isr in : ");
//    Serial.println(ISRVector,BIN);
/// detachInterrupt(32);      // docasne zakazat isr - ale nepomohlo
///  device[ISRdev].digitalRead();      // s tim to havaruje
  ISRVector = device[ISRdev].getPorts();    // nenacita stav vstupů
  // Serial.println(ISRVector, HEX);

//    Serial.print("ISRVector in isr red: ");
//    Serial.println(ISRVector,BIN);

    interrupt = true;
//    readInputs(ISRdev);     // s tim havaruje
//    Serial.println(ISRVector);
//    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á

  digitalWrite(LEDPIN, ledState);     // POZOR - dlouhá loop - dlouhá odezva - ošetřit jinak
//    Serial.print("ISRVector in isr ex: ");
//    Serial.println(ISRVector,BIN);
  }
  attachInterrupt(Add0, doISR, CHANGE);   //znovu povolit isr
}

void setup() {

  pinMode(Add0, INPUT_PULLUP);
  pinMode(34, INPUT_PULLUP);
  pinMode(LEDPIN, OUTPUT);

  Wire.begin();
  Serial.begin(115200);
  Serial.println("\nI2C Scanner");

  int nDevices;
  Serial.println("Scanning...");
  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");
  }

//set for auto-increnent
///reg = mode1;
data = 160;           // 0b10100000  viz Mode register 1 PCA9685 str.13   RESTART a AUTOINCREMENT
//data = 176;           // 0b11100000  viz Mode register 1 PCA9685 str.13   RESTART, SLEEP a AUTOINCREMENT
//data = 32;           // 0b00100000  viz bit 5 Mode register 1 PCA9685 str.13

  // 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();
*/



/***===================================================================***/
//  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
    pwm2.setPWM(servoNum, 0, getAnglePulse(stopAngle));
  }

/***===================================================================***/


  FastLED.addLeds<WS2812, DATA_PIN, GRB>(leds, NUM_LEDS);  // GRB ordering is typical 

  attachInterrupt(Add0, doISR, CHANGE);

}
 
void loop() {

 //  device[ISRdev].digitalRead();
  //  ISRVector = device[ISRdev].getPorts();

    Serial.print("ISRVector from isr: ");
    Serial.println(ISRVector, HEX);

    ISR_Xvector = XreadInputs(ISRdev);
    Serial.print("ISRX_Vector from readInputs : ");
    Serial.println(ISR_Xvector, HEX);

    Serial.print("var interrupt : ");
    Serial.println(interrupt);
/* zpracovat flag if(interrupt=true) a pak ho vynulovat  */
    interrupt = false;
    Serial.print("var ledState : ");
    Serial.println(ledState);

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



  for(uint8_t idx = 0; idx<7; idx++){
    Serial.print("inputs on device : ");
    Serial.print(idx, HEX);
    Serial.print(" bin : ");
    Serial.println(getChangedInputValue(idx), BIN);
  }

//      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);
//      Serial.println("===== mirror 6 =====");
      //delay(20);
///      mirrorInputOntoOutputs(5);
/*
      Serial.println("ISR vectors on device(7)");
      //delay(20);
      readInputs(7);
///      mirrorInputOntoOutputs(7);
*/


leds[0]    = CRGB::Orange;
leds[1]    = CRGB::Green;
leds[2]    = CRGB::Red;
leds[3]    = CRGB::Black;
//...
leds[6]    = CRGB::Blue;
leds[7]    = CRGB::White;

FastLED.show();
delay(200);


/***====================================================================***/
    // 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
    }
*/
//
/***====================================================================***/



/*  //ALL_LED
ledN = 61;  //odkaz na registr 250  ALL_LED
led_on = 4095;
led_off = 4096;     // k tomu musi byt  led_on = 4095 jinak nestiha simulace
WRITE_LED(0x40, ledN);
*/

leds[0]    = CRGB::Orange;
leds[1]    = CRGB::Black;
leds[2]    = CRGB::Black;
leds[3]    = CRGB::Orange;
//...
leds[6]    = CRGB::White;
leds[7]    = CRGB::Blue;

FastLED.show();

delay(200);
//delay(1000);

}

/***********************************************************/

void LED_SW(uint8_t PCA_addr, uint16_t ledN, uint16_t led_on, uint16_t led_off){
if (led_stat[ledN] == 0) {
   led_stat[ledN] = 1;
   led_on = 4096;
   led_off = 0;
   WRITE_LED(PCA_addr, ledN, led_on, led_off);
  }
else{
   led_stat[ledN] = 0;
   led_on = 0;
   led_off = 4096;
   WRITE_LED(PCA_addr, ledN, led_on, led_off);
  }
}

void SetOFF() {
led_on = 0;
led_off = 4096;
WRITE_LED;
}

void READREG(uint8_t dev_addr) {
Wire.beginTransmission( dev_addr );
Wire.write(reg);
error = Wire.endTransmission();

Wire.beginTransmission( dev_addr );
Wire.requestFrom(dev_addr,1);     //start a transaction to read 1 byte
rval = Wire.read();               //read the byte
error = Wire.endTransmission();
}

void WRITE_LED(uint8_t dev_addr, uint8_t ledN, uint16_t led_on, uint16_t led_off){
byte ah,al;
Wire.beginTransmission( dev_addr );
Wire.write(led0 + 4*ledN);
ah = led_on >> 8;
al = led_on && 255;
Wire.write(al);
Wire.write(ah);
ah = led_off >> 8;
al = led_off and 255;
Wire.write(al);
Wire.write(ah);
error = Wire.endTransmission();
//Serial.print("error > ");
/*
Serial.print(error);
Serial.print(" on addr 0x");
Serial.print(dev_addr,HEX);
Serial.print(" LED:");
Serial.print(ledN);
Serial.print(" led_on :");
Serial.print(led_on);
Serial.print(" led_off :");
Serial.println(led_off);
*/
}

void WRITE_REG(uint8_t dev_addr, uint8_t reg, uint8_t data ){
Wire.beginTransmission( dev_addr );
Wire.write(reg);
Wire.write(data);
error = Wire.endTransmission();
}
/**************************************************************************/

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 0x");
    Serial.print(newValue, HEX);
    Serial.print(" on device ");
    Serial.println((int)idx);
//     if(newValue < 0xFFFE)
//      device[idx].digitalWrite (0xFFFE);
//      else device[idx].digitalWrite (0xFFFF);
  }

uint16_t XreadInputs(uint8_t idx) {
    uint16_t newValue = getChangedInputValue(idx);
    Serial.print("Inputs Xread 0x");
    Serial.print(newValue, HEX);
    Serial.print(" on device ");
    Serial.println((int)idx);
//     if(newValue < 0xFFFE)
//      device[idx].digitalWrite (0xFFFE);
//      else device[idx].digitalWrite (0xFFFF);
    return newValue;  
  }