//init button 1
const int buttonPin1 = p20;
const int relay1 = p22;
int oldValue1 = HIGH; // default/idle value for pin 4 is high.
// end of button 1 init

//init button 2
const int buttonPin2 = p21;
const int relay2 = p26;
int oldValue2 = HIGH; // default/idle value for pin 4 is high.
// end of button 1 init

int newValue1 = HIGH;
int newValue2 = HIGH;


int chaincounter = 0; // should be in NVRAM.....
float chainlength = 0.3; // Default value = 30 cm. should be in NVRAM.....

const byte ReedRelay = p6;  // input from winch: magnet + reed sensor
int Reed = 0; // Reed relay pulse count

void setup() 
{
  Serial1.begin(115200);
  Serial1.println("Hello, Raspberry Pi Pico!");
  Serial1.println("Press a button.");

  // Initialize the pin for reading button 1.
  pinMode(buttonPin1, INPUT_PULLUP);
  pinMode(relay1, OUTPUT);
  digitalWrite(relay1, LOW);

  // Initialize the pin for reading button 2.
  pinMode(buttonPin2, INPUT_PULLUP);
  pinMode(relay2, OUTPUT);
  digitalWrite(relay2, LOW);

  // initialize the pin for reed sensor
  pinMode(ReedRelay, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ReedRelay), readEncoder, FALLING);

}

void loop() 
{
  int nosetup = 0;  // normally we are not in setup mode....
  int keypressed = 0; // 0= no key, 1 = up, 2 = down, 3 = both keys --> setup

  newValue1 = digitalRead(buttonPin1);
  newValue2 = digitalRead(buttonPin2);
  keypressed = (newValue1 == LOW) + (2*(newValue2 == LOW));

  //////////////////////////////////////////////////////////
  //****************** Menu Setup **************************
  //////////////////////////////////////////////////////////
  if (keypressed == 3){ // Enter in setup mode if the two keys are pressed
    digitalWrite(relay1, LOW);  //stop up & down operations
    digitalWrite(relay2, LOW);
  }
  
  // Check if the value was changed,
  // by comparing it with the previous value.
  if(newValue1 != oldValue1)  // button 1 pressed
  {
    if(newValue1 == LOW)  digitalWrite(relay1, HIGH);
    else  digitalWrite(relay1, LOW);
    // Remember the value for the next time.
    oldValue1 = newValue1;
  }

  if(newValue2 != oldValue2)  // button 2 pressed
  {
    if(newValue2 == LOW)  digitalWrite(relay2, HIGH);
    else  digitalWrite(relay2, LOW);
    // Remember the value for the next time.
    oldValue2 = newValue2;
  }

  // chaincounter is updated automatically on each winch turn. we only have to care about the direction (up or down) 
  if (Reed ==1){  // winch turn read via interrupt
    // increase chain counter if DOWN pressed OR winch turn (in case of free fall), decreas if UP is pressed AND winch turn
    if(oldValue2 == LOW){ // Down pressed and winch turn
      chaincounter +=1;
    }
    if(oldValue1 == LOW){   // up pressed and winch turn
      chaincounter -=1;
    }
    if((oldValue2 == HIGH) && (oldValue1 == HIGH)){ // No button pressed but winch turn: free fall
      chaincounter +=1;
    }
    // chaincounter = chaincounter + ((oldValue2 == LOW) - (oldValue1 == LOW));
    Reed = 0;
  }

  Serial1.print("Compteur:"); Serial1.println(round(chaincounter*chainlength), 0); // for debugging only
   // Slow down for debouncing the buttons.
  delay(100);
}

void readEncoder() {
  Reed =1;
  Serial1.print("Reed relay ⏩"); Serial1.println(Reed); // for debugging only
}