#include <SPI.h>
#include <Wire.h>
// #include <Adafruit_GFX.h>
// #include <SSD1306Ascii.h>
#include "SSD1306Ascii.h"
#include "SSD1306AsciiWire.h"
#include <Arduino.h>
#include <EEPROM.h>
#include <AsyncTimer.h>
#include <inttypes.h>
#include <SmartButton.h>


#define DEFAULT_PUMP_ON_DURATION    5000
#define DEFAULT_BUZZER_DELAY        4000
#define DEFUALT_PUMP_OFF_DURATION   10000
#define MIN_DURATION                500
#define MAX_DURATION                300000
#define PARAMETER_INCREMENT         500
#define DEFAULT_REPETITIONS_NUMBER  10
#define MIN_REPETITIONS_NUMBER      1
#define MAX_REPETITIONS_NUMBER      100
#define STARTUP_BUZZER_FREQUENCY    1000
#define BUZZER_FREQUENCY            4000
#define EEPROM_INITIAL_ADDRESS      4
#define VALIDITY_CHECK_VALUE        0xABCD

#define PARAMETERS_NUMBER            4

#define PUMP_ON_DURATION_INDEX      0
#define BUZZER_DELAY_INDEX          1
#define PUMP_OFF_DURATION_INDEX     2
#define REPETITIONS_NUMBER_INDEX    3

#define LED_PIN 13
#define BUZZER_PIN 7

const char STATIC_STRING_0[] PROGMEM = "Pump On Duration";
const char STATIC_STRING_1[] PROGMEM = "Buzzer Delay";
const char STATIC_STRING_2[] PROGMEM = "Pump Off Duration";
const char STATIC_STRING_3[] PROGMEM = "Repetitions Number";

PGM_P const STATIC_STRINGS[] PROGMEM =
{
    STATIC_STRING_0,
    STATIC_STRING_1,
    STATIC_STRING_2,
    STATIC_STRING_3
};

#define FIELD_WIDTH 5

//typedef String (*ParameterConverter)(uint32_t);
typedef const char* (*ParameterConverter)(uint32_t, uint8_t);

class Configuration;
template<class T>
class Parameter {
  friend class Configuration;

  uint8_t id_;
  T value_;
  T minValue_;
  T maxValue_;
  T increment_;
  bool printAsFloat_;

  bool initialized_;
public:
  Parameter() : id_(0xFF), value_(0), minValue_(0), maxValue_(0), increment_(0), initialized_(false), printAsFloat_(false) {}

  Parameter(uint8_t id, const T& value, const T& minValue, const T& maxValue, const T& increment, bool printAsFloat) : 
    id_(id),
    value_(value), 
    minValue_(minValue), 
    maxValue_(maxValue), 
    increment_(increment),
    initialized_(true),
    printAsFloat_(printAsFloat) {}

  Parameter(uint8_t id, const T& value, bool printAsFloat) : 
    id_(id),
    value_(value), 
    minValue_(MIN_DURATION), 
    maxValue_(MAX_DURATION), 
    increment_(PARAMETER_INCREMENT),
    initialized_(true),
    printAsFloat_(printAsFloat) {}
    
  Parameter(const Parameter&) = default;
  Parameter& operator=(const Parameter&) = default;
  virtual ~Parameter() = default;

  bool operator==(const Parameter& right) const {
    return id_ == right.id_ &&
      value_ == right.value_ &&
      minValue_ == right.minValue_ &&
      maxValue_ == right.maxValue_ &&
      increment_ == right.increment_ &&
      printAsFloat_ == right.printAsFloat_;
  }

  bool operator!=(const Parameter& right) const {
    return !operator==(right);
  }

  uint8_t getId() const {
    return id_;
  }

  void setId(uint8_t id) {
    id_ = id;
  }

  // String getName() const {
  //   // Get first string in the instructions array
  //   char buffer[32];
  //   strcpy_P(buffer, (PGM_P)pgm_read_word(&STATIC_STRINGS[id_]));
  //   return String(buffer);
  // }

  const char* getName() const {
    // Get first string in the instructions array
    static char buffer[20];
    strcpy_P(buffer, (PGM_P)pgm_read_word(&STATIC_STRINGS[id_]));
    return buffer;
  }

  T getValue() const {
    return value_;
  }

  void setValue(const T& value) {
    value_ = value;
  }

  T& operator++() {
    if (value_ + increment_ <= maxValue_) {
      value_+=increment_;
    }

    return value_;
  }

  // T operator++(int) {
  //   if (value_ + increment_ <= maxValue_) {
  //     value_+=increment_;
  //   }

  //   return value_;
  // }

  T& operator--() {
    if (value_ - increment_ >= minValue_) {
      value_-=increment_;
    }
    
    return value_;
  }

  // T operator--(int) {
  //   if (value_ - increment_ >= minValue_) {
  //     value_-=increment_;
  //   }

  //   return value_;
  // }

  bool isValid() const {
    return initialized_ && value_ >= minValue_ && value_ <= maxValue_;
  }

  static double convertToDouble(uint32_t value) {
    double tmp = value;
    tmp /= 1000;
    return tmp;
  }

  //static String convert(uint32_t x) {
  static const char* convert(uint32_t x, uint8_t fieldWidth = FIELD_WIDTH) {
    static char paramBuffer[16];
    memset(paramBuffer, 0, sizeof(paramBuffer));
    dtostrf(convertToDouble(x), fieldWidth, 1, paramBuffer);
    // Serial.print(F("paramBuffer: "));
    // Serial.println(paramBuffer);
    // Serial.println(String(paramBuffer).c_str());
    //return String(paramBuffer);
    return paramBuffer;
  };

  //static String convertRaw(uint32_t x) {
  static const char* convertRaw(uint32_t x, uint8_t fieldWidth = FIELD_WIDTH) {
    static char paramBuffer[16];
    char format[16];
    sprintf(format, "%c%d%c", '%', fieldWidth, 'd');
    sprintf(paramBuffer, format, x);

    //return String(paramBuffer);
    return paramBuffer;
  };

  //template<typename Functor>
  //String toString(ParameterConverter converter, bool printName = false) const {
  const char* toString(ParameterConverter converter, uint8_t fieldWidth, bool printName = false) const {
    if (printName) {
      char buffer[64];
      //sprintf(buffer, "%s: %s", getName().c_str(), converter(value_).c_str());
      //sprintf(buffer, "%s: %s", getName(), converter(value_).c_str());
      sprintf(buffer, "%s: %s", getName(), converter(value_, fieldWidth));
      //Serial.println(buffer);
      //return String(buffer);
      return buffer;
    } else {
      //Serial.println(converter(value_).c_str());
      return converter(value_, fieldWidth);
    }    
  }

  bool isInitialized() const {
    return initialized_;
  }

  void reset() {
    initialized_ = false;
  }

  void setMinValue(T minValue) {
    minValue_ = minValue;
  }

  void setMaxValue(T maxValue) {
    maxValue_ = maxValue;
  }

  void setIncrement(T increment) {
    increment_ = increment;
  }

  void setInitialized(bool init) {
    initialized_ = init;
  }

  bool isPrintAsFloat() const {
    return printAsFloat_;
  }

  void setPrintAsFloat(bool printAsFloat) {
    printAsFloat_ = printAsFloat;
  }
};

struct ConfigurationData_t {
  uint16_t validityCheck_;
  uint32_t pumpOnDuration_;
  uint32_t buzzerDelay_;
  uint32_t pumpOffDuration_;
  uint16_t repetitionsNumber_;

  ConfigurationData_t() : 
    validityCheck_(0xABCD), 
    pumpOnDuration_(DEFAULT_PUMP_ON_DURATION), 
    buzzerDelay_(DEFAULT_BUZZER_DELAY), 
    pumpOffDuration_(DEFUALT_PUMP_OFF_DURATION),
    repetitionsNumber_(DEFAULT_REPETITIONS_NUMBER) {}
};

class ConfigurationManager;
class Configuration {
  friend class ConfigurationManager;
  uint16_t validityCheck_;
  Parameter<uint32_t> parameters_[PARAMETERS_NUMBER];
public:
  Configuration();
  Configuration(uint16_t validityCheck, const Parameter<uint32_t>* parameters);
  Configuration(const ConfigurationData_t& confData);
  Configuration(const Configuration& right);
  Configuration& operator=(const Configuration& right);
  virtual ~Configuration() = default;

  bool operator==(const Configuration& right) const;
  bool operator!=(const Configuration& right) const;

  uint16_t getValidityCheck() const;
  void setValidityCheck(uint16_t validityCheck);  
  Parameter<uint32_t> getParameter(uint8_t index) const;
  void setParameter(uint8_t index, const Parameter<uint32_t>& param);
  void incrementParameter(uint8_t index);
  void decrementParameter(uint8_t index);

  bool isValid() const;

  ConfigurationData_t getConfigurationData() const;

  const char* getParameterAsString(uint8_t paramIndex, uint8_t fieldWidth, bool printName = false);
  const char* getRawParameterAsString(uint8_t paramIndex, uint8_t fieldWidth, bool printName = false);
  const char* formatParameter(uint8_t paramIndex, uint8_t fieldWidth, bool printName = false);
};

class ConfigurationManager {
  static ConfigurationManager* instance_;
  ConfigurationManager() = default;
  
  Configuration configuration_;

public:
  static ConfigurationManager* getInstance();
  bool loadConfig();

  ConfigurationManager(const ConfigurationManager&) = delete;
  ConfigurationManager& operator=(const ConfigurationManager&) = delete;
  virtual ~ConfigurationManager() = default;

  bool isValid() const;

  static double convert(uint32_t value);
  // static String toString(const Parameter<uint32_t>& parameter, bool printName = false); 
  // String getParameterAsString(uint8_t paramIndex, bool printName = false);
  // String getRawParameterAsString(uint8_t paramIndex, bool printName = false);

  // String formatParameter(uint8_t paramIndex, bool printName = false);
  static const char* toString(const Parameter<uint32_t>& parameter, uint8_t fieldWidth, bool printName = false); 
  const char* getParameterAsString(uint8_t paramIndex, uint8_t fieldWidth, bool printName = false);
  const char* getRawParameterAsString(uint8_t paramIndex, uint8_t fieldWidth, bool printName = false);

  const char* formatParameter(uint8_t paramIndex, uint8_t fieldWidth, bool printName = false);

  Configuration getConfiguration() const;
  void setConfiguration(const Configuration&);

  //String getParameterName(uint8_t paramIndex) const;
  const char* getParameterName(uint8_t paramIndex) const;

  void printConfigurationData();

};

static const uint32_t PARAMETERS_DEFAULT_VALUES[] = {DEFAULT_PUMP_ON_DURATION, DEFAULT_BUZZER_DELAY, DEFUALT_PUMP_OFF_DURATION, DEFAULT_REPETITIONS_NUMBER};

Configuration::Configuration() : 
  validityCheck_(VALIDITY_CHECK_VALUE),
  parameters_({
    Parameter<uint32_t>(0, PARAMETERS_DEFAULT_VALUES[0], true),
    Parameter<uint32_t>(1, PARAMETERS_DEFAULT_VALUES[1], true),
    Parameter<uint32_t>(2, PARAMETERS_DEFAULT_VALUES[2], true),
    Parameter<uint32_t>(3, PARAMETERS_DEFAULT_VALUES[3], MIN_REPETITIONS_NUMBER, MAX_REPETITIONS_NUMBER, 1, false)
  }
  ) {
  }

Configuration::Configuration(uint16_t validityCheck, const Parameter<uint32_t>* parameters) {
  validityCheck_ = validityCheck;

  for (int i=0; i<PARAMETERS_NUMBER; i++) {
    parameters_[i] = parameters[i];
  }
}

Configuration::Configuration(const ConfigurationData_t& confData) {
  Parameter<uint32_t> tmpParameters[PARAMETERS_NUMBER] = {
    Parameter<uint32_t>(0, confData.pumpOnDuration_, true),
    Parameter<uint32_t>(1, confData.buzzerDelay_, true),
    Parameter<uint32_t>(2, confData.pumpOffDuration_, true),
    Parameter<uint32_t>(3, confData.repetitionsNumber_, MIN_REPETITIONS_NUMBER, MAX_REPETITIONS_NUMBER, 1, false)
  };

  validityCheck_ = confData.validityCheck_;
  for (int i=0; i<PARAMETERS_NUMBER; i++) {
    parameters_[i] = tmpParameters[i];
  }
}

Configuration::Configuration(const Configuration& right) {
  validityCheck_ = right.validityCheck_;
  for (int i=0; i<PARAMETERS_NUMBER; i++) {
    parameters_[i] = right.parameters_[i];
  }
}

Configuration& Configuration::operator=(const Configuration& right) {
  if (this != &right) {
    validityCheck_ = right.validityCheck_;
    for (int i=0; i<PARAMETERS_NUMBER; i++) {
      parameters_[i] = right.parameters_[i];
    }
  }

  return *this;
}

bool Configuration::operator==(const Configuration& right) const {
  bool result = validityCheck_ == right.validityCheck_;

  if (result) {
    for (int i=0; i<PARAMETERS_NUMBER; i++) {
      result = parameters_[i] == right.parameters_[i];
      if (!result) {
        break;
      }
    }
  }

  return result;
}

bool Configuration::operator!=(const Configuration& right) const {
  return !operator==(right);
}

Parameter<uint32_t> Configuration::getParameter(uint8_t index) const {
  if (index < PARAMETERS_NUMBER) {
    return parameters_[index];
  }

  return Parameter<uint32_t>();
}

uint16_t Configuration::getValidityCheck() const {
  return validityCheck_;
}

void Configuration::setValidityCheck(uint16_t validityCheck) {
  validityCheck_ = validityCheck;
}

void Configuration::setParameter(uint8_t index, const Parameter<uint32_t>& param) {
  if (index < PARAMETERS_NUMBER) {
    parameters_[index] = param;
  }
}

bool Configuration::isValid() const {
  bool result = validityCheck_ == VALIDITY_CHECK_VALUE;
  //Serial.print(F("validityCheck: "));
  //Serial.println(validityCheck_);

  if (result) {
    for (int i=0; i<PARAMETERS_NUMBER; i++) {
      result = parameters_[i].isValid();

      if (!result) {
        break;
      }
    }
  }

  return result;
}

void Configuration::incrementParameter(uint8_t index) {
  if (index < PARAMETERS_NUMBER) {
    ++parameters_[index];
  }
}

void Configuration::decrementParameter(uint8_t index) {
  if (index < PARAMETERS_NUMBER) {
    --parameters_[index];
  }
}

ConfigurationData_t Configuration::getConfigurationData() const {
  ConfigurationData_t result;

  result.validityCheck_ = validityCheck_;
  result.pumpOnDuration_ = parameters_[PUMP_ON_DURATION_INDEX].value_;
  result.buzzerDelay_ = parameters_[BUZZER_DELAY_INDEX].value_;
  result.pumpOffDuration_ = parameters_[PUMP_OFF_DURATION_INDEX].value_;
  result.repetitionsNumber_ = parameters_[REPETITIONS_NUMBER_INDEX].value_;

  return result;
}

const char* Configuration::getParameterAsString(uint8_t paramIndex, uint8_t fieldWidth, bool printName) {
  static const char* result = "";
  
  if (paramIndex < PARAMETERS_NUMBER) {
    //result = toString(parameters_[paramIndex], printName);
    result = parameters_[paramIndex].toString(&Parameter<uint32_t>::convert, fieldWidth, printName);
  }

  return result;
}

const char* Configuration::getRawParameterAsString(uint8_t paramIndex, uint8_t fieldWidth, bool printName) {
  static const char* result = "";
  
  if (paramIndex < PARAMETERS_NUMBER) {   
    result = parameters_[paramIndex].toString(&Parameter<uint32_t>::convertRaw, fieldWidth, printName);
  }

  return result;
}

const char* Configuration::formatParameter(uint8_t paramIndex, uint8_t fieldWidth, bool printName) {
  static const char* result = "";
  
  if (paramIndex < PARAMETERS_NUMBER) {   
    if (parameters_[paramIndex].isPrintAsFloat()) {
      result = getParameterAsString(paramIndex, fieldWidth, printName);
    } else {
      result = getRawParameterAsString(paramIndex, fieldWidth, printName);
    }
  }

  return result;
}

ConfigurationManager* ConfigurationManager::instance_ = nullptr;

ConfigurationManager* ConfigurationManager::getInstance() {
  if (instance_ == nullptr) {
    instance_ = new ConfigurationManager();
  } 

  return instance_;
}

bool ConfigurationManager::loadConfig() {
  bool result = false;

  ConfigurationData_t configurationData;
  //Variable to store custom object read from EEPROM.
  EEPROM.get(EEPROM_INITIAL_ADDRESS, configurationData);
  Configuration tmpConfiguration(configurationData);
  
  if (tmpConfiguration.isValid()) {
    Serial.println(F("Configuration validity check result: ok"));
  } else {
    Serial.println(F("Configuration validity check result: not ok"));
    Serial.println(F("Writing default values..."));
    ConfigurationData_t defaultConfig;
    EEPROM.put(EEPROM_INITIAL_ADDRESS, defaultConfig);
    
    Serial.println(F("Default values correctly written in EEPROM"));
  EEPROM.get(EEPROM_INITIAL_ADDRESS, configurationData);
  Configuration tmpConfiguration(configurationData);
    if (tmpConfiguration.isValid()) {
      Serial.println(F("Configuration validity check result 1: ok"));
    }
    configuration_ = Configuration(defaultConfig);
  }
  
  return result;
}

bool ConfigurationManager::isValid() const {
  return configuration_.isValid();
}

// double ConfigurationManager::convert(uint32_t value) {
//   double tmp = value;
//   tmp /= 1000;
//   return tmp;
// }

//String ConfigurationManager::toString(const Parameter<uint32_t>& parameter, bool printName) {
const char* ConfigurationManager::toString(const Parameter<uint32_t>& parameter, uint8_t fieldWidth, bool printName) {
  return parameter.toString(&Parameter<uint32_t>::convert, fieldWidth, printName);
}

//String ConfigurationManager::getParameterAsString(uint8_t paramIndex, bool printName) {
const char* ConfigurationManager::getParameterAsString(uint8_t paramIndex, uint8_t fieldWidth, bool printName) {
  static const char* result = "";
  
  if (paramIndex < PARAMETERS_NUMBER) {
    result = toString(configuration_.parameters_[paramIndex], fieldWidth, printName);
    //Serial.println(result.c_str());
  }

  return result;
}

//String ConfigurationManager::getRawParameterAsString(uint8_t paramIndex, bool printName) {
const char* ConfigurationManager::getRawParameterAsString(uint8_t paramIndex, uint8_t fieldWidth, bool printName) {
  static const char* result = "";
  
  if (paramIndex < PARAMETERS_NUMBER) {   
    // auto lambda = [] (uint32_t x) {
    //   char paramBuffer[16];
    //   char format[16];
    //   sprintf(format, "%c%d%c", '%', FIELD_WIDTH, 'd');
    //   sprintf(paramBuffer, format, x);
      
    //   return String(paramBuffer);
    // };

    //result = configuration_.parameters_[paramIndex].toString(lambda, printName);
    result = configuration_.parameters_[paramIndex].toString(&Parameter<uint32_t>::convertRaw, fieldWidth, printName);
  }

  return result;
}

//String ConfigurationManager::formatParameter(uint8_t paramIndex, bool printName) {
const char* ConfigurationManager::formatParameter(uint8_t paramIndex, uint8_t fieldWidth, bool printName) {
  static const char* result = "";
  
  if (paramIndex < PARAMETERS_NUMBER) {   
    if (configuration_.parameters_[paramIndex].isPrintAsFloat()) {
      result = getParameterAsString(paramIndex, printName);
    } else {
      result = getRawParameterAsString(paramIndex, fieldWidth, printName);
    }
  }

  return result;
}

void ConfigurationManager::printConfigurationData() {
  for (int i=0; i<PARAMETERS_NUMBER; i++) {
    //Serial.println(formatParameter(i, true).c_str());
    Serial.println(formatParameter(i, 1, true));
  }
}

Configuration ConfigurationManager::getConfiguration() const {
  return configuration_;
}

void ConfigurationManager::setConfiguration(const Configuration& configuration) {
  configuration_ = configuration;
}

//String ConfigurationManager::getParameterName(uint8_t paramIndex) const {
const char* ConfigurationManager::getParameterName(uint8_t paramIndex) const {
  //String result = "";
  static char* result = "";
  
  if (paramIndex < PARAMETERS_NUMBER) {
    result = configuration_.parameters_[paramIndex].getName();  
  }

  return result;
}

#include <SmartButton.h>

using namespace smartbutton;

#define CONFIG_TIMEOUT 7000

enum class ButtonObject : uint8_t {
  CONFIG,
  INCREMENT,
  DECREMENT,
  START_STOP
};

class State;

class Context {
  State* currentState_ = nullptr;
  SSD1306Ascii* display_ = nullptr;
public:
  Context(SSD1306Ascii* display) : display_(display) {}
  Context(const Context&) = delete;
  Context& operator=(const Context&) = delete;
  virtual ~Context() = default;

  void setState(State* state);

  void handle();

  void refreshDisplay();

  void handleEvent(ButtonObject button, SmartButton::Event event, int clickCounter);

};

class State {
protected:
  Context* context_ = nullptr;
  static AsyncTimer stateTimer_;

  void clearValue(SSD1306Ascii* display, uint8_t col0, uint8_t col1, uint8_t row);
  
  //static State* instance_;
  State() = default;
public:
  
  State(const State&) = delete;
  State& operator=(const State&) = delete;
  virtual ~State() = default;
  virtual void handleButtonPressed(ButtonObject button) {};
  virtual void handleButtonReleased(ButtonObject button) {};
  virtual void handleButtonClick(ButtonObject button, int clickCounter) {};
  virtual void handleButtonHold(ButtonObject button) {};
  virtual void handleButtonLongHold(ButtonObject button) {};
  virtual void handleButtonHoldRepeat(ButtonObject button) {};
  virtual void handleButtonLongHoldRepeat(ButtonObject button) {};

  virtual void onExit();
  virtual void onEnter();

  virtual void display(SSD1306Ascii* display) {};

  void setContext(Context* context);

  void handle();

  //static void drawCentreString(SSD1306Ascii* display, const String &buf, int x, int y);
  static void drawCentreString(SSD1306Ascii* display, const char* buf, uint8_t yOffset = 0);
};
AsyncTimer State::stateTimer_ = AsyncTimer(3);
class InitState : public State {
  friend class State;

  InitState() = default;
  InitState(const InitState&) = delete;
  InitState& operator=(const InitState&) = delete;

  static InitState* instance_;
public:
  virtual ~InitState() = default;
  static InitState* getInstance();

  virtual void handleButtonClick(ButtonObject button, int clickCounter) override;
  virtual void handleButtonHold(ButtonObject button) override;

  virtual void display(SSD1306Ascii* display) override;
  virtual void onExit() override;
};

class ConfigViewState : public State {
  friend class State;

  ConfigViewState() = default;
  ConfigViewState(const ConfigViewState&) = delete;
  ConfigViewState& operator=(const ConfigViewState&) = delete;

  unsigned short timerId_ = 0;
  static ConfigViewState* instance_;
public:
  virtual ~ConfigViewState() = default;
  static ConfigViewState* getInstance();

  virtual void handleButtonClick(ButtonObject button, int clickCounter) override;
  virtual void handleButtonHold(ButtonObject button) override;

  virtual void onEnter() override;
  virtual void display(SSD1306Ascii* display) override;
  virtual void onExit() override;
};

class RunningState : public State {
  friend class State;

  RunningState() = default;
  RunningState(const RunningState&) = delete;
  RunningState& operator=(const RunningState&) = delete;

  unsigned short buzzerTimerId_ = 0;
  unsigned short stopTimerId_ = 0;
  unsigned short refreshTimerId_ = 0;

  static RunningState* instance_;
  uint8_t currentIteration_ = 0;
  uint8_t maxIterations_ = 0;
  unsigned long timestamp_ = 0;
  bool pumpOn_ = false;
  bool titleDrawn_ = false;

  void runCycle();

public:
  virtual ~RunningState() = default;
  static RunningState* getInstance();

  virtual void handleButtonClick(ButtonObject button, int clickCounter) override;
  virtual void display(SSD1306Ascii* display) override;
  virtual void onEnter() override;
  virtual void onExit() override;
};

class ConfigEditState : public State {
  friend class State;

  bool holdFilter_ = true;
  uint8_t parameterIndex_ = -1;
  bool show_ = true;
  bool titleDrawn_ = false;
  Configuration currentConfiguration_;
  
  ConfigEditState() = default;
  ConfigEditState(const ConfigEditState&) = delete;
  ConfigEditState& operator=(const ConfigEditState&) = delete;

  unsigned short timerId_ = 0;
  unsigned short refreshTimerId_ = 0;
  static ConfigEditState* instance_;
public:
  virtual ~ConfigEditState() = default;
  static ConfigEditState* getInstance();

  virtual void handleButtonClick(ButtonObject button, int clickCounter) override;
  virtual void handleButtonHold(ButtonObject button) override;
  virtual void handleButtonReleased(ButtonObject button) override;

  virtual void onEnter() override;
  virtual void display(SSD1306Ascii* display) override;
  virtual void onExit() override;
};

#define REFRESH_DISPLAY_TIMEOUT 100

void Context::setState(State* state) {
  Serial.println(F("Context::setState"));
  if (currentState_ != nullptr) {
    currentState_->onExit();

    //delete currentState_;
  }

  state->setContext(this);
  
  currentState_ = state;

  state->onEnter();
}

void Context::handle() {
  currentState_->handle();
}

void Context::handleEvent(ButtonObject button, SmartButton::Event event, int clickCounter) {
  switch(event) {
    case SmartButton::Event::RELEASED:
      currentState_->handleButtonReleased(button);
      break;
    case SmartButton::Event::PRESSED:
      currentState_->handleButtonPressed(button);
      break;
    case SmartButton::Event::CLICK:
      currentState_->handleButtonClick(button, clickCounter);
      break;
    case SmartButton::Event::HOLD:
      currentState_->handleButtonHold(button);
      break;
    case SmartButton::Event::HOLD_REPEAT:
      currentState_->handleButtonHoldRepeat(button);
      break;
    case SmartButton::Event::LONG_HOLD:
      currentState_->handleButtonLongHold(button);
      break;
    case SmartButton::Event::LONG_HOLD_REPEAT:
      currentState_->handleButtonLongHoldRepeat(button);
      break;
  }
}

void Context::refreshDisplay() {
  //Serial.println(F("Context::refreshDisplay"));
  if (currentState_ != nullptr) {
    currentState_->display(display_);
  }
}

void State::setContext(Context* context) {
  this->context_ = context;
}

void State::onEnter() {
  Serial.println(F("State::onEnter"));
  this->context_->refreshDisplay();
}

void State::onExit() {
  //Serial.println(F("State::onExit"));
  //stateTimer_.cancelAll();
}

void State::handle() {
  stateTimer_.handle();
}

//void State::drawCentreString(SSD1306Ascii* display, const String &buf, int x, int y)
void State::drawCentreString(SSD1306Ascii* display, const char* buf, uint8_t yOffset)
{
    size_t w = display->strWidth(buf);
    //display->setCursor(display->displayWidth()/2 - w / 2, display->fontRows() + yOffset);
    display->setCursor(display->displayWidth()/2 - w / 2, display->fontRows() - yOffset);
    display->println(buf);
}


void State::clearValue(SSD1306Ascii* display, uint8_t col0, uint8_t col1, uint8_t row) {
  uint8_t rows = display->fontRows();
  display->clear(col0, col1, row, row + rows - 1);
}

InitState* InitState::instance_ = nullptr;

InitState* InitState::getInstance() {
  //return State::getInstance<InitState>();
  if (instance_ == nullptr) {
    instance_ = new InitState();
  } 

  return instance_;
}

void InitState::handleButtonClick(ButtonObject button, int clickCounter) {
  switch(button) {
    case ButtonObject::CONFIG:
      context_->setState(ConfigViewState::getInstance());
      break;
    case ButtonObject::START_STOP:
      context_->setState(RunningState::getInstance());
      break;
    default:
      break;
  }
}

void InitState::handleButtonHold(ButtonObject button) {
  if (button == ButtonObject::CONFIG) {
    context_->setState(ConfigEditState::getInstance());
  }
}

void InitState::display(SSD1306Ascii* display) {
  Serial.println(F("InitState::display"));
  display->clear();

  //display->setTextSize(1);             // Normal 1:1 pixel scale
  display->set1X();
  //display->setTextColor(SSD1306_WHITE);        // Draw white text
  display->setCursor(0,display->displayHeight()-display->fontRows());             
  display->println(F("Config          Start"));
  
  //display->display();
}

void InitState::onExit() {
  Serial.println(F("InitState::onExit"));
  //stateTimer_.cancelAll();
  //stateTimer_.cancel()
}

ConfigViewState* ConfigViewState::instance_ = nullptr;

ConfigViewState* ConfigViewState::getInstance() {
  //return State::getInstance<ConfigViewState>();
  if (instance_ == nullptr) {
    instance_ = new ConfigViewState();
  } 

  return instance_;
}

void ConfigViewState::handleButtonClick(ButtonObject button, int clickCounter) {
  if (button == ButtonObject::CONFIG) {
    context_->setState(InitState::getInstance());
  }
}

void ConfigViewState::handleButtonHold(ButtonObject button) {
  if (button == ButtonObject::CONFIG) {
    context_->setState(ConfigEditState::getInstance());
  }
}

void ConfigViewState::onEnter() {
  Serial.println(F("ConfigViewState::onEnter"));
  State::onEnter();
  timerId_ = stateTimer_.setTimeout([=]() { context_->setState(InitState::getInstance()); }, CONFIG_TIMEOUT);
}

void ConfigViewState::display(SSD1306Ascii* display) {
  Serial.println(F("ConfigViewState::display"));
  display->clear();

  display->set1X();             // Normal 1:1 pixel scale
  //display->setTextColor(SSD1306_WHITE);        // Draw white text
  display->home();             // Start at top-left corner
  //display.println(F("  RD   BD   SD    RN"));
  display->println(F("  ON   BD   OFF   RN"));
  //Serial.println(ConfigurationManager::getInstance()->getParameterAsString(PUMP_ON_DURATION_INDEX).c_str());
  // display->print(ConfigurationManager::getInstance()->getParameterAsString(PUMP_ON_DURATION_INDEX).c_str());
  // display->print(ConfigurationManager::getInstance()->getParameterAsString(BUZZER_DELAY_INDEX).c_str());
  // display->print(ConfigurationManager::getInstance()->getParameterAsString(PUMP_OFF_DURATION_INDEX).c_str());
  // display->print(ConfigurationManager::getInstance()->getRawParameterAsString(REPETITIONS_NUMBER_INDEX).c_str());
  display->print(ConfigurationManager::getInstance()->getParameterAsString(PUMP_ON_DURATION_INDEX, FIELD_WIDTH));
  display->print(ConfigurationManager::getInstance()->getParameterAsString(BUZZER_DELAY_INDEX, FIELD_WIDTH));
  display->print(ConfigurationManager::getInstance()->getParameterAsString(PUMP_OFF_DURATION_INDEX, FIELD_WIDTH));
  display->print(ConfigurationManager::getInstance()->getRawParameterAsString(REPETITIONS_NUMBER_INDEX, FIELD_WIDTH));

  //display->display();
}

void ConfigViewState::onExit() {
  Serial.println(F("ConfigViewState::onExit"));
  //stateTimer_.cancelAll();
  stateTimer_.cancel(timerId_);
}

RunningState* RunningState::instance_ = nullptr;

RunningState* RunningState::getInstance() {
  //return State::getInstance<RunningState>();
  if (instance_ == nullptr) {
    instance_ = new RunningState();
  } 

  return instance_;
}

void RunningState::handleButtonClick(ButtonObject button, int clickCounter) {
  if (button == ButtonObject::START_STOP) {
    context_->setState(InitState::getInstance());
  }
}

void RunningState::runCycle() {
  Configuration currentConfiguration = ConfigurationManager::getInstance()->getConfiguration();

  // TODO 
  // Start Buzzer
  // Start Pump
  // Turn Len On
  Serial.println(F("START BUZZER ON"));
  tone(BUZZER_PIN, STARTUP_BUZZER_FREQUENCY);

  /*buzzerTimerId_ = stateTimer_.setTimeout([=]() { 
    // TODO 
    // Start Buzzer
    Serial.println(F("START BUZZER OFF"));
    noTone(BUZZER_PIN);
  }, 500);*/
  delay(300);
  noTone(BUZZER_PIN);

  digitalWrite(LED_PIN, HIGH);
  maxIterations_ = currentConfiguration.getParameter(REPETITIONS_NUMBER_INDEX).getValue();
  this->timestamp_ = millis();
  this->pumpOn_ = true;

  context_->refreshDisplay();
  
  //refreshTimerId_ = stateTimer_.setInterval([=]() { context_->refreshDisplay(); }, REFRESH_DISPLAY_TIMEOUT);
  buzzerTimerId_ = stateTimer_.setTimeout([=]() { 
      // TODO 
      // Start Buzzer
      Serial.println(F("BUZZER ON"));
      tone(BUZZER_PIN, BUZZER_FREQUENCY);

      buzzerTimerId_ = stateTimer_.setTimeout([=]() { 
        // TODO 
        // Start Buzzer
        Serial.println(F("BUZZER OFF"));
        noTone(BUZZER_PIN);
      }, 500);
    }, 
    currentConfiguration.getParameter(BUZZER_DELAY_INDEX).getValue());

  stopTimerId_ = stateTimer_.setTimeout([=]() { 
    // TODO 
    // Stop Pump
    // Turn Len Off
    digitalWrite(LED_PIN, LOW);
    Serial.println(F("PUMP STOP"));
    this->pumpOn_ = false;
    this->timestamp_ = millis();
    stopTimerId_ = stateTimer_.setTimeout([=]() { 
        Serial.println(F("NEXT ITERATION")); 
        if (currentIteration_ < maxIterations_) {
          ++currentIteration_;
          context_->refreshDisplay();
          runCycle();
        } else {
          context_->refreshDisplay();
          delay(1000);
          context_->setState(InitState::getInstance());
        }
      }, 
      currentConfiguration.getParameter(PUMP_OFF_DURATION_INDEX).getValue());
    }, 
    currentConfiguration.getParameter(PUMP_ON_DURATION_INDEX).getValue());
}

void RunningState::onEnter() {
  State::onEnter();
  this->titleDrawn_ = false;
  this->pumpOn_ = false;
  this->timestamp_ = 0;
  currentIteration_ = 1;

  runCycle();
  refreshTimerId_ = stateTimer_.setInterval([=]() { context_->refreshDisplay(); }, REFRESH_DISPLAY_TIMEOUT);
}

#define CLEAR_WIDTH 30
void RunningState::onExit() {
  Serial.println(F("RunningState::onExit"));
  //stateTimer_.cancelAll();
  stateTimer_.cancel(buzzerTimerId_);
  stateTimer_.cancel(stopTimerId_);
  stateTimer_.cancel(refreshTimerId_);
  digitalWrite(LED_PIN, LOW);
  noTone(BUZZER_PIN);
}

void RunningState::display(SSD1306Ascii* display) {
  //Serial.println(F("RunningState::display"));
  //display->clear();
  display->home();
  display->set1X();             // Normal 1:1 pixel scale
  if (!titleDrawn_) {
    titleDrawn_ = true;
    display->clear();
    display->home();             // Start at top-left corner
    //display.println(F("  RD   BD   SD    RN"));
    display->println(F("Iteration    : "));
    display->println(F("Pump On  Time: "));
    display->println(F("Pump Off Time: "));
  }
  uint8_t width = display->strWidth("Iteration    : ");
  //Serial.println(width);

  unsigned long currentTime = millis();
  unsigned long delta = currentTime - timestamp_;
  clearValue(display, width, width+CLEAR_WIDTH, 0);
  display->setCursor(width, 0);
  display->print(this->currentIteration_);
  display->print(F("/")); 
  display->print(this->maxIterations_);
  if (this->pumpOn_) {
    double tmp = (double)delta/1000;
    clearValue(display, width, width+CLEAR_WIDTH, 1);
    display->setCursor(width, display->fontRows());
    display->print(tmp, 1);
    clearValue(display, width, width+CLEAR_WIDTH, 2);
    display->setCursor(width, display->fontRows()*2);
    display->print(F("---"));
  } else {
    double tmp = (double)delta/1000;
    clearValue(display, width, width+CLEAR_WIDTH, 1);
    display->setCursor(width, display->fontRows());
    display->print(F("---"));
    clearValue(display, width, width+CLEAR_WIDTH, 2);
    display->setCursor(width, display->fontRows()*2);
    display->print(tmp, 1);    
  }
}

ConfigEditState* ConfigEditState::instance_ = nullptr;

ConfigEditState* ConfigEditState::getInstance() {
  //return State::getInstance<ConfigEditState>();
  if (instance_ == nullptr) {
    instance_ = new ConfigEditState();
  } 

  return instance_;
}

void ConfigEditState::onEnter() {
  State::onEnter();
  this->holdFilter_ = true;
  this->titleDrawn_ = false;
  parameterIndex_ = 0;
  currentConfiguration_ = ConfigurationManager::getInstance()->getConfiguration();
  timerId_ = stateTimer_.setTimeout([=]() { context_->setState(InitState::getInstance()); }, CONFIG_TIMEOUT);
  refreshTimerId_ = stateTimer_.setInterval([=]() { context_->refreshDisplay(); }, 4*REFRESH_DISPLAY_TIMEOUT);
}

void ConfigEditState::display(SSD1306Ascii* display) {
  if (!titleDrawn_) {
    display->clear();
  }

  display->set1X();             // Normal 1:1 pixel scale
  //display->setTextColor(SSD1306_WHITE);        // Draw white text
  display->home();             // Start at top-left corner
  if (!titleDrawn_) {
    drawCentreString(display, ConfigurationManager::getInstance()->getParameterName(parameterIndex_), 1);
    titleDrawn_ = true;
  }
  //drawCentreString(display, currentConfiguration_.getParameterName(parameterIndex_));
  //display->setCursor(8,8);
  
  display->set2X();
  //Serial.println(parameterIndex_);
  if (show_) {
    // drawCentreString(display, ConfigurationManager::getInstance()->formatParameter(parameterIndex_));
    drawCentreString(display, currentConfiguration_.formatParameter(parameterIndex_, 1));
    //display->println(ConfigurationManager::getInstance()->formatParameter(parameterIndex_));
  } else {
    //drawCentreString(display, "     ");
    display->setCursor(0, display->fontRows());
    display->clearToEOL();
  }
  
  //display->display();

  show_ = !show_;
}

void ConfigEditState::handleButtonHold(ButtonObject button) {
  if (button == ButtonObject::CONFIG) {
    if (!this->holdFilter_) {
      this->holdFilter_ = true;
      // save
      Serial.println(F("Checking differences..."));
      Configuration savedConfiguration = ConfigurationManager::getInstance()->getConfiguration();
      if (currentConfiguration_ != savedConfiguration) {
        Serial.println(F("Saving data to EEPROM..."));
        EEPROM.put(EEPROM_INITIAL_ADDRESS, currentConfiguration_.getConfigurationData());
        Serial.println(F("Data saved to EEPROM..."));
        ConfigurationManager::getInstance()->setConfiguration(currentConfiguration_);
      } else {
        Serial.println(F("No change occurred"));
      }

      context_->setState(InitState::getInstance());
    }
  }
}

void ConfigEditState::handleButtonReleased(ButtonObject button) {
  stateTimer_.reset(timerId_);
  switch(button) {
    case ButtonObject::CONFIG:
      // next param
      if (!holdFilter_) {
        //for (int i=0; i<clickCounter; ++i) {
          parameterIndex_++;
          parameterIndex_ = parameterIndex_ % PARAMETERS_NUMBER;
          this->titleDrawn_ = false;
          context_->refreshDisplay();
        //}
        stateTimer_.reset(refreshTimerId_);
      }
      //context_->refreshDisplay();
      break;
    case ButtonObject::INCREMENT:
      // param++
      //for (int i=0; i<clickCounter; ++i) {
        currentConfiguration_.incrementParameter(parameterIndex_);
        show_ = true;
        context_->refreshDisplay();
      //}
      stateTimer_.reset(refreshTimerId_);
      //context_->refreshDisplay();
      break;
    case ButtonObject::DECREMENT:
      // param--
      //for (int i=0; i<clickCounter; ++i) {
        currentConfiguration_.decrementParameter(parameterIndex_);
        show_ = true;
        context_->refreshDisplay();
      //}
      stateTimer_.reset(refreshTimerId_);
      //context_->refreshDisplay();
      break;
    default:
      break;
  }
  if (button == ButtonObject::CONFIG) {
    this->holdFilter_ = false;
    stateTimer_.reset(refreshTimerId_);
    context_->refreshDisplay();
  }
}

void ConfigEditState::handleButtonClick(ButtonObject button, int clickCounter) {
  /*stateTimer_.reset(timerId_);
  switch(button) {
    case ButtonObject::CONFIG:
      // next param
      for (int i=0; i<clickCounter; ++i) {
        parameterIndex_++;
        parameterIndex_ = parameterIndex_ % PARAMETERS_NUMBER;
        this->titleDrawn_ = false;
        context_->refreshDisplay();
      }
      stateTimer_.reset(refreshTimerId_);
      //context_->refreshDisplay();
      break;
    case ButtonObject::INCREMENT:
      // param++
      for (int i=0; i<clickCounter; ++i) {
        currentConfiguration_.incrementParameter(parameterIndex_);
        show_ = true;
        context_->refreshDisplay();
      }
      stateTimer_.reset(refreshTimerId_);
      //context_->refreshDisplay();
      break;
    case ButtonObject::DECREMENT:
      // param--
      for (int i=0; i<clickCounter; ++i) {
        currentConfiguration_.decrementParameter(parameterIndex_);
        show_ = true;
        context_->refreshDisplay();
      }
      stateTimer_.reset(refreshTimerId_);
      //context_->refreshDisplay();
      break;
    default:
      break;
  }*/
}

void ConfigEditState::onExit() {
  Serial.println(F("ConfigEditState::onExit"));
  //stateTimer_.cancelAll();
  stateTimer_.cancel(timerId_);
  stateTimer_.cancel(refreshTimerId_);
}

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
// The pins for I2C are defined by the Wire-library. 
// On an arduino UNO:       A4(SDA), A5(SCL)
// On an arduino MEGA 2560: 20(SDA), 21(SCL)
// On an arduino LEONARDO:   2(SDA),  3(SCL), ...
#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
//SSD1306Ascii display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
SSD1306AsciiWire display;
Context context(&display);

#define CONFIG_BUTTON_PIN 8
#define INCREMENT_BUTTON_PIN 9
#define DECREMENT_BUTTON_PIN 10
#define START_STOP_BUTTON_PIN 11

using namespace smartbutton;
// class ButtonInterface: public SmartButtonInterface {
// public:
//     virtual void event(SmartButton *button, SmartButton::Event event, int clickCounter)
//     {
//         context.handleEvent(ButtonObject::CONFIG, event);
//     }

//     virtual bool isPressed(SmartButton *button)
//     {
//         return (digitalRead(BUTTON_PIN) == LOW) ? true : false;
//     }
// };

// ButtonInterface buttonInterface;

SmartButton confButton(CONFIG_BUTTON_PIN, SmartButton::InputType::NORMAL_HIGH);

//SmartButton button(&buttonInterface);
SmartButton incButton(INCREMENT_BUTTON_PIN, SmartButton::InputType::NORMAL_HIGH);
SmartButton decButton(DECREMENT_BUTTON_PIN, SmartButton::InputType::NORMAL_HIGH);
SmartButton runButton(START_STOP_BUTTON_PIN, SmartButton::InputType::NORMAL_HIGH);

void eventCallback(SmartButton *button, SmartButton::Event event, int clickCounter)
{
  ButtonObject obj = ButtonObject::CONFIG;
  if (button == &confButton) {
    obj = ButtonObject::CONFIG;
  } else if (button == &incButton) {
    obj = ButtonObject::INCREMENT;
  } else if (button == &decButton) {
    obj = ButtonObject::DECREMENT;
  } else if (button == &runButton) {
    obj = ButtonObject::START_STOP;
  }

  context.handleEvent(obj, event, clickCounter);
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(19200);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }

  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  // if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
  //   Serial.println(F("SSD1306 allocation failed"));
  //   for(;;); // Don't proceed, loop forever
  // }
  display.begin(&Adafruit128x32, SCREEN_ADDRESS);
  display.setFont(System5x7);
  display.clear();

  Serial.println(F("Checking configuration..."));
  ConfigurationManager::getInstance()->loadConfig();
  ConfigurationManager::getInstance()->printConfigurationData();

  // Show initial display buffer contents on the screen --
  // the library initializes this with an Adafruit splash screen.
  //display.display();
  delay(2000); // Pause for 2 seconds

  // Clear the buffer
  display.clear();

  pinMode(CONFIG_BUTTON_PIN, INPUT_PULLUP);  // Digital input with pull-up resistors (normal high)
  pinMode(INCREMENT_BUTTON_PIN, INPUT_PULLUP);  // Digital input with pull-up resistors (normal high)
  pinMode(DECREMENT_BUTTON_PIN, INPUT_PULLUP);  // Digital input with pull-up resistors (normal high)
  pinMode(START_STOP_BUTTON_PIN, INPUT_PULLUP);  // Digital input with pull-up resistors (normal high)

  pinMode(LED_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  //button.begin();                     // Initialize and register smart button

  confButton.begin(eventCallback);                     // Initialize and register smart button
  incButton.begin(eventCallback);                     // Initialize and register smart button
  decButton.begin(eventCallback);                     // Initialize and register smart button
  runButton.begin(eventCallback);                     // Initialize and register smart button

  context.setState(InitState::getInstance());
}

void loop() {
  // put your main code here, to run repeatedly:
  context.handle();
  SmartButton::service();             // Asynchronous service routine, should be called periodically
}