// screen init
#include <Wire.h>
#include "SSD1306Ascii.h"
#include "SSD1306AsciiWire.h"

// 0X3C+SA0 - 0x3C or 0x3D
#define I2C_ADDRESS 0x3C

// Define proper RST_PIN if required.
#define RST_PIN -1

SSD1306AsciiWire oled;
// end of screen init

//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 splash = 0; 

#define MENU_SIZE 3
char *menu[MENU_SIZE] = { "RAZ compteur", "long. / tour", "Exit"};
int cursor = 0;

int chaincounter = 0; // should be in NVRAM.....
float chainlength = 0.3; // 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);

  //// setup screen
  Wire.begin();
  Wire.setClock(400000L);

 #if RST_PIN >= 0
  oled.begin(&Adafruit128x64, I2C_ADDRESS, RST_PIN);
 #else // RST_PIN >= 0
  oled.begin(&Adafruit128x64, I2C_ADDRESS);
 #endif // RST_PIN >= 0
  
  oled.clear();
  oled.setFont(System5x7);
  oled.setCursor(0, 0);
  oled.print("Compteur de chaine:");
  oled.setCursor(40, 1);
  oled.print("Pret!");
  oled.setCursor(0, 3);
  oled.print("Left = UP");
  oled.setCursor(0, 5);
  oled.print("Right = DOWN");
  oled.setCursor(0, 7);
  oled.print("Left + Right = MENU");
   //// end of screen setup

}

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));

  if (keypressed > 0){
    if (splash == 0){ // cleanup splash screen on the first key pressed
    splash = 1;
    oled.clear();
    oled.setCursor(0,7);
    oled.print("UP               DOWN");
    newValue1 = HIGH;
    newValue2 = HIGH;
    } 
  }

  //////////////////////////////////////////////////////////
  //****************** 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);
    oled.clear();
    nosetup = 1;
    for(int i=0; i < 20; i++){  // wait 2 seconds before entering setup
      if ((digitalRead(buttonPin1) == HIGH) && (digitalRead(buttonPin2) == HIGH)){
        nosetup = 0;
        newValue1 = HIGH;
        newValue2 = HIGH;
        break;
      } 
      delay(100);
    }

    if(nosetup == 1){
      showMenu();

      for(;;){

       while((digitalRead(buttonPin1) == LOW) && (digitalRead(buttonPin2) == LOW)){
         delay(100); // wait until all buttons are released
        }

        int choice = (digitalRead(buttonPin1) == LOW);
        int exec = (digitalRead(buttonPin2) == LOW);

        if (exec == 1){ // button 2 pressed - execute the choice
          exec = 0;
          if(cursor == (MENU_SIZE -1)){ // EXIT setup is selected : return to normal operations
            oled.clear();
            oled.setCursor(0,7);
            oled.print("UP               DOWN");
            newValue1 = HIGH;
            newValue2 = HIGH;
            break;
          }  //exit setup
          executeChoice(cursor);
          while(digitalRead(buttonPin2) == LOW){
            delay(100); // wait until button 2 is released
          }

        }
        if (choice == 1){ //button 1 pressed - set cursor on next menu entry
          oled.setCursor(0,cursor*2);
          oled.print(' ');
          cursor = cursor +1;
          if (cursor >= MENU_SIZE) cursor = 0;
          oled.setCursor(0,cursor*2);
          oled.print('>');
          choice = 0;
          while(digitalRead(buttonPin1) == LOW){
            delay(100); // wait until button 1 is released
          }
        }
        delay(100);
      }
    }
  } //*************** exit setup *****************
  
  // 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;
  }
  if(splash == 1){  // if current mode is normal operation, display chain counter in big chars.
    oled.setCursor(40, 2);
//    oled.setFont(lcdnums14x24);
    oled.setFont(fixednums15x31);
    oled.print(round(chaincounter*chainlength), 0); oled.print("   ");
    oled.setFont(System5x7);
    oled.setCursor(0,7);
    oled.print("UP               DOWN");
  }
  // Slow down for debouncing the buttons.
  delay(100);
}

//*****************************************
//*** Clear display and show the Setup menu.
//******************************************
void showMenu() {

  oled.clear();
  oled.setFont(System5x7);
  
  // show menu items:
  for (int i = 0; i<MENU_SIZE; i++) {
    oled.setCursor(10, i*2);
    oled.print(menu[i]);  
    if( i == 0) {
      oled.print(" (");
      oled.print(round(chaincounter*chainlength), 0);
      oled.print(")");
    }
    if( i == 1) {
      oled.print(" (");
      oled.print(chainlength);
      oled.print(")");
    }

  }
  oled.setCursor(0,7);
  oled.print("Choix              OK");
  oled.setCursor(0,0);
  oled.print('>');
  cursor = 0;
}

//*****************************************
//*** Compute the chain lenght
//******************************************
void circonference() {

  oled.clear();
  oled.setFont(System5x7);
  oled.setCursor(0, 0);
  oled.print("longueur de chaine"); 
  oled.setCursor(0, 2);
  oled.print("pour un tour de"); 
  oled.setCursor(0, 4);
  oled.print("guindeau:");
  oled.setCursor(50, 6);
  oled.print(chainlength);
  oled.setCursor(0,7);
  oled.print("Choix              OK");

  while(digitalRead(buttonPin2) == LOW){
    delay(100); // wait until all buttons are released
  }
  
  // increase or decrease chain length
  for (;;) {    
    if(digitalRead(buttonPin1) == LOW){ // increase the length of the chain (for one turn)
      chainlength += 0.01;              // 1 cm for each button press
      if (chainlength > 0.90) chainlength = 0.10; // chain length must be between 10 and 90 cm.
      oled.setCursor(50,6);
      oled.print(chainlength);
      while((digitalRead(buttonPin1) == LOW)){  // prevent automatic increase by long pressing the button
        delay(100); // wait until button 1 is released
      }
    }
    
    if(digitalRead(buttonPin2) == LOW){
      break;
    }
    delay(100);
  }
}

//*********************************************************
//*** Execute the task which matches the chosen menu item.
//*********************************************************
void executeChoice(int choice) {
  switch(choice) {
      case 0 :  // reset chain counter
        Serial1.print("Execute choice "); Serial1.print(choice); Serial1.print(" - "); Serial1.println(menu[choice]);
        chaincounter = 0;
        oled.setCursor(10, 0);
        oled.print(menu[0]);  
        oled.print(" (");
        oled.print(round(chaincounter*chainlength), 0);
        oled.print(")    ");
        break;
      case 1 :  // set chain length
        Serial1.print("Execute choice "); Serial1.print(choice); Serial1.print(" - "); Serial1.println(menu[choice]);
        circonference();
        // reset also the chain counter
        chaincounter = 0;
        showMenu();
        break;
      case 2 :
                Serial1.print("Execute choice "); Serial1.print(choice); Serial1.print(" - "); Serial1.println(menu[choice]);
                break;
      case 3 :
                Serial1.print("Execute choice "); Serial1.print(choice); Serial1.print(" - "); Serial1.println(menu[choice]);
                break;
      case 4 :
                Serial1.print("Execute choice "); Serial1.print(choice); Serial1.print(" - "); Serial1.println(menu[choice]);
                break;
      default :
                Serial1.print("Execute choice "); Serial1.print(choice); Serial1.print(" - "); Serial1.println(menu[choice]);
                break;
  }
  
}

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