/***********************************************************************************
* Authors   :   HaaMuPaPa
* Version   :   2.0
* Date      :   14.12.2023
*
* 
*  Timber controller simulation
*  Not tested with real ESP
***********************************************************************************/

/***********************************************************************************
 * BLE server attributes
 **********************************************************************************/

// Connection check
bool deviceConnected     = false;
bool restartOnDisconnect = false;

// Connection LED
const int connectionLED = 12;

// Timer attributes (to avoid delay) used for controlling ble notify
long currentMillis;
long notifyZeroMillis;
bool zeroNotSent = false;

const int defaultZeroInterval = 200;
int zeroInterval;  // This changes if using joystick

// Connection based delay
int variableDelay = 100;

// Data to be sent
int mask;


/***********************************************************************************
 * Buttons
 **********************************************************************************/

// Button Bytes
enum buttonBytes {
  Red     = 0b0001,
  Yellow  = 0b0010,
  Green   = 0b0100,
  Blue    = 0b1000,

  Up      = 0b0001 << 4,
  Down    = 0b0010 << 4,
  Right   = 0b0100 << 4,
  Left    = 0b1000 << 4
};

// Colorbutton pins
const int red    = 26;
const int yellow = 27;
const int green  = 13;
const int blue   = 14;

// Joystick pins
const int up     = 23;
const int down   = 32;
const int right  = 33;
const int left   = 25;

// Arrays for looping
const byte   buttonCount              = 8;
const byte   byteArray[buttonCount]   = {Red, Yellow, Green, Blue, Up, Down, Right, Left};
const byte   buttons[buttonCount]     = {red, yellow, green, blue, up, down, right, left};
const String buttonNames[buttonCount] = {"Red", "Yellow", "Green", "Blue", "Up", "Down", "Right", "Left"};

// Arrays for togglebutton logic
int buttonLastStates[buttonCount];
int buttonStates[buttonCount];


//################################################################################//
// Functions
//################################################################################//

void setup() {
  
  Serial.begin(115200);
  Serial.println("ServerESP");

  /*********************************************************************************
   * Setup pinmodes, states
   ********************************************************************************/

  // Buttons
  for ( int i = 0; i < buttonCount; i++ ) {
    pinMode(buttons[i], INPUT_PULLUP);

    buttonLastStates[i] = HIGH;
    buttonStates[i] = HIGH;
  }

  // Connection LED
  pinMode(connectionLED, OUTPUT);
  digitalWrite(connectionLED, LOW);


  deviceConnected = true;                                              // Wokwi only
}


// Function to read buttons, returns masked byte
int readButtons() {
  
  /*********************************************************************************
    OR bit operations for multiple pressed buttons

    PULL_UP works in reverse (0 = 1, 1 = 0): use !digitalRead()

    0 = LOW = false
    1 = HIGH = true
   ********************************************************************************/
  
  // Initialize mask
  mask = 0;

  // Loop trough buttonStates. If pressed, operate mask with corresponding byte
  for ( int i = 0; i < buttonCount; i++ ){
    if ( !digitalRead(buttons[i]) == HIGH ){
      mask = mask | byteArray[i];
    }
  }

  return mask;
}



// Main loop
void loop() {

  if ( deviceConnected ) {

    // Set ESP to work fast
    variableDelay = 1;

    // On disconnect reset ESP
    restartOnDisconnect = true;

    // Turn on LED
    digitalWrite(connectionLED, HIGH);

    // Get mask value
    mask = readButtons();

    // Send registered value if not already pressed
    /*******************************************************************************
      Button notify loop
      Do once / Togglebutton logic
     
      PULL_UP works in reverse (0 = 1, 1 = 0): use !digitalRead()

      Laststate = low   &&  currentstate = high -> button being pressed
      Laststate = high  &&  currentstate = high -> button still pressed
      Laststate = high  &&  currentstate = low  -> button released
      Laststate = low   &&  currentstate = low  -> button still released
     ******************************************************************************/

    for ( int i = 0; i < buttonCount; i++ ) {

      buttonLastStates[i] = buttonStates[i];           // Save current to last state
      buttonStates[i]     = !digitalRead(buttons[i]);  // Read new state to current

      // Do once
      if (buttonLastStates[i] == LOW && buttonStates[i] == HIGH) {
        Serial.println("Button pressed: " + buttonNames[i]);                // Debug

        Serial.println("Set value, notify");                           // Wokwi only

      }
    }

    // Send zero mask when no button is pressed
    /*******************************************************************************
      Bitshift to check Joystick bits only
      Sets zeroNotify lower -> zeroNotify is sent faster after joystick is released
      making joystick more responsive without spamming BLE connection
    ******************************************************************************/

    if ( mask >> 4 > 0 ) {
      zeroInterval = 30;
    }

    // Activate and reset zerotimer if not zero
    if ( mask != 0 ) {
      zeroNotSent = true;
      notifyZeroMillis = millis();
    }

    currentMillis = millis();

    // Buttons "stays pressed" for zeroInterval of time
    if ( mask == 0 
         && zeroNotSent 
         && currentMillis - notifyZeroMillis >= zeroInterval ) {

      // reset timer attributes to default
      zeroNotSent = false;
      zeroInterval = defaultZeroInterval;

      Serial.println("Button released");                                    // Debug

      Serial.println("Set value, notify");                             // Wokwi only

      notifyZeroMillis = millis();
    }

  }

  // No connection
  else {
    
    // Slowdown ESP
    variableDelay = 100;

    // Turn off LED
    digitalWrite(connectionLED, LOW);
    
    // Reset ESP to make it advertise again
    if (restartOnDisconnect) {
      ESP.restart();
    }

  }
  
  delay(variableDelay);

  delay(5);                                                            // Wokwi only
}