#include <Wire.h>
#include <U8g2lib.h>
#include <EEPROM.h>

U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);

#define MS5611_ADDRESS 0x77
#define DIAL_PIN A0
#define SWITCH_PIN A1
#define RELAY_PIN 9
#define VBAT_PIN A3


//#define FONT_NAME u8g2_font_fub14_tr
//#define LINEHEIGHT 20

#define FONT_NAME u8g2_font_fub11_tr
#define LINEHEIGHT 14


unsigned int calibrationData[6];

void twiSendCommand(byte address, byte command)
{
  Wire.beginTransmission(address);  
  Wire.write(command);
  Wire.endTransmission();
}

void twiGetValue(byte command, byte numBytes, byte* dest)
{
  twiSendCommand(MS5611_ADDRESS, command);
  Wire.requestFrom(MS5611_ADDRESS, numBytes);
  byte index = numBytes - 1;
  for (byte i = 0; i < numBytes; i++)
    dest[index--] = Wire.read();
}

void getMS5611Data(uint32_t& D1, uint32_t& D2)
{  
  D1 = D2 = 0;

  twiSendCommand(MS5611_ADDRESS, 0x48); // start pressure conversion
  delay(10);
  twiGetValue(0x00, 3, (byte*)&D1);
  
  twiSendCommand(MS5611_ADDRESS, 0x50); // start temperature conversion
  delay(1);
  twiGetValue(0x00, 3, (byte*)&D2);
}

float fmap(float x, float in_min, float in_max, float out_min, float out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

bool getSwitchState() {
  return analogRead( SWITCH_PIN) < 512;
}

float getDialValue( int mapMin, int mapMax ) {
  return fmap( analogRead(DIAL_PIN), 3, 1020, mapMin, mapMax);
}

void printFloat( float f, float d, char* buf, char* format ) {
  int whole = f/d;
  int decimal = ((f/d) - whole) * 10;
  sprintf(buf, format, whole, decimal );
}

struct _settingParam {
  char* name;
  int minVal; // real units
  int maxVal; // real units
  unsigned int val; // 0 - 65535
};

enum _page {
  ST_MIN_VOLTAGE,
  ST_MAX_TEMP,
  ST_MAX_TIME,
  ST_PRESSURE_MARGIN,
  ST_PRESSURE // normal running mode
};

_settingParam settings[] = {
  { "Min volt",      8,  13, 0 },
  { "Max temp C",   12,  35, 0 },
  { "Max time s",    1,  30, 0 },
  { "Margin mbar",  20, 100, 0 },
  { "Pressure",    400, 950, 0 },
};

float getSettingValue( int which ) {
  _settingParam& param = settings[which];
  return fmap( param.val, 0, 65535, param.minVal, param.maxVal );
}

byte currentPage = 0;
char buf[24];
byte line = 0;
unsigned long lastPageChangeTime = 0;
bool awaitingNextPageTrigger = false;

void startPage() {
  u8g2.clearBuffer();
  line = LINEHEIGHT;
}

void doLine(char* str) {
  u8g2.drawStr(0,line,str);
  line += LINEHEIGHT;
}

void endPage() {
  u8g2.sendBuffer();
}


void setup()
{
  Wire.begin();
  twiSendCommand(MS5611_ADDRESS, 0x1E);
  delay(100);

  //Serial.begin(115200);
  
  for ( byte i = 0; i < 6; i++) {
    byte cmd = 0xA0 + (i+1) * 2; // calibration data
    twiGetValue(cmd, 2, (byte*)&calibrationData[i]);
  }

  pinMode( RELAY_PIN, OUTPUT );
  digitalWrite( RELAY_PIN, LOW );

  u8g2.begin();
  u8g2.setFont(FONT_NAME);
  u8g2.setFlipMode(0);

  if ( getSwitchState() ) {
    currentPage = 0;
    startPage();  
    doLine( "Enter settings" );
    endPage();
    while ( getSwitchState() ) ;
  }
  else {
    for (byte i = 0; i < ST_PRESSURE; i++) {
      _settingParam& param = settings[i];
      param.val = (int)EEPROM.read(2*i) + (((int)EEPROM.read(2*i+1)) << 8);
    }

    startPage();
  
    printFloat( getSettingValue(ST_MIN_VOLTAGE), 1, buf, "MinV: %d.%d" );
    doLine( buf );
    printFloat( getSettingValue(ST_MAX_TEMP), 1, buf, "Temp: %d.%d" );
    doLine( buf );
    printFloat( getSettingValue(ST_MAX_TIME), 1, buf, "Time: %d.%d" );
    doLine( buf );
    printFloat( getSettingValue(ST_PRESSURE_MARGIN), 1, buf, "Margin: %d.%d" );
    doLine( buf );
    
    endPage();

    delay( 5000 );
    
    currentPage = ST_PRESSURE;    
  }
}


void loop_settings() {

  if ( awaitingNextPageTrigger ) {
    if ( ! getSwitchState() ) {
      currentPage++;
      awaitingNextPageTrigger = false;
      if ( currentPage >= ST_PRESSURE )
        return; 
    }
  }

  _settingParam& param = settings[currentPage];
  float val = getDialValue( param.minVal, param.maxVal );
  
  startPage();
  doLine( param.name );
  printFloat(val, 1, buf, "  %d.%d" );
  doLine( buf );
  if ( awaitingNextPageTrigger )
    doLine("  Saved");
  endPage();

  if ( ! awaitingNextPageTrigger ) {
    if ( getSwitchState() ) {
      param.val = (unsigned int)map(val, param.minVal, param.maxVal, 0, 65535);
      EEPROM.write(2*currentPage, param.val & 0x00FF );
      EEPROM.write(2*currentPage+1, param.val >> 8 );
      awaitingNextPageTrigger = true;
    }
  }
}

bool pumpOn = false;
unsigned long lastPumpStartedTime = 0;
unsigned long lastPumpStoppedTime = 0;

void setPumpStatus(bool tf) {
  if ( tf && !pumpOn )
    lastPumpStartedTime = millis();
  else if ( !tf && pumpOn )
    lastPumpStoppedTime = millis();  
  pumpOn = tf;
  digitalWrite( RELAY_PIN, pumpOn );
}


void loop()
{
  if ( currentPage < ST_PRESSURE ) {
    loop_settings();
    return;
  }
  
  uint32_t D1, D2;
  getMS5611Data(D1, D2);
  
  // names as given on page 8 of datasheet
  int32_t dT;
  int64_t OFF, SENS;
  long P, TEMP;

  dT = D2 - ((long)calibrationData[4] << 8);
  TEMP = 2000 + (((int64_t)dT * (int64_t)calibrationData[5]) >> 23);
  OFF = ((unsigned long)calibrationData[1] << 16) + (((int64_t)calibrationData[3] * dT) >> 7);
  SENS = ((unsigned long)calibrationData[0] << 15) + (((int64_t)calibrationData[2] * dT) >> 8);
  P = ((D1 * (SENS >> 21)) - OFF) >> 15;


  bool switchOn = getSwitchState();
  int dialPercent = getDialValue(0, 100);

  float r1 = 5.34;
  float r2 = 10.579;
  float vbat = ((float)analogRead( VBAT_PIN )/1023.0) * 5.0 * (( r1 + r2 ) / r1);

  _settingParam& param = settings[currentPage];
  float desiredPressure = getDialValue( param.minVal, param.maxVal );
  

  unsigned long now = millis();

  bool tempOk = (TEMP/100.0) < getSettingValue(ST_MAX_TEMP);
  bool vbatOk = vbat > getSettingValue(ST_MIN_VOLTAGE);

  bool timeOk = false;

  if ( ! pumpOn ) {
    timeOk = (now - lastPumpStoppedTime) > (2000 * getSettingValue(ST_MAX_TIME));
    bool allowPump = getSwitchState() && timeOk && vbatOk && tempOk && ((P / 100.0) > desiredPressure + getSettingValue(ST_PRESSURE_MARGIN));
    setPumpStatus( allowPump );
  }  
  else if ( pumpOn ) {
    timeOk = (now - lastPumpStartedTime) < 1000 * getSettingValue(ST_MAX_TIME);
    bool allowPump = getSwitchState() && timeOk && vbatOk && tempOk && ((P / 100.0) > desiredPressure - getSettingValue(ST_PRESSURE_MARGIN));
    setPumpStatus( allowPump );
  }

  startPage();

  printFloat(desiredPressure, 1, buf, "Goal: %d.%d" );
  doLine( buf );
  printFloat(P, 100, buf, "mbar: %d.%d" );
  doLine( buf );
  printFloat(TEMP, 100, buf, "Temp: %d.%d" );
  doLine( buf );
  printFloat(vbat, 1, buf, "vbat: %d.%d" );
  doLine( buf );
  
  endPage();

}