// include the library code:
#include <LiquidCrystal.h>
#include <math.h>
#include <SimpleKalmanFilter.h>


int currentLevel = 0;
int lastLevel = 0;
int tankDepth = 104 * 58;
int maxDepth = 85 * 58;
int minDepth = 20 * 58;
//int tankDepth = 104;
//int maxDepth = 85;
//int minDepth = 20;
const int tankEmptyDepth = 104; // This MUST be no greater than 450 cm (500cm for the HC-SR04)!
const int tankFullDepth = 20; // This should be at least 25 cm, if possible (2cm for the HC-SR04)


// These vars are for showPartialBarChar() and showCurrentLevel()
int done = 0;

char timeoutErrorTxt[] = "Sensor Disconect";

// Var for echo response from ultrasonic sensor board
unsigned long timeHigh;

bool turnMotorOn = false;
bool turnMotorOff = false;
long turnMotorOnPulseEndAt = 0;
long turnMotorOffPulseEndAt = 0;
long stateSince = 0;
long lastStateDepth = 0;
long idealDepthIncreasePer30s = tankDepth / 45 / 2;
long microsecondsToCentimeters(long microseconds) {
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the object we
  // take half of the distance travelled.
  return microseconds / 29 / 2;
}

// micros
long depth, durationRaw;
long depthCm;
long refreshSerial = 0;

#define STATE_FILLING 1
#define STATE_UNKNOWN 0
#define STATE_EMPTYING 2
#define STATE_NO_ELECTRICITY 4
#define STATE_WAITING_FOR_WATER 5
#define STATE_SENSOR_DISCONNECT 6

//boolens
bool ledHigh = false;
bool isFilled() {
  return depth <= minDepth;
}

bool isEmpty() {
  return depth > maxDepth;
}

bool isPartial() {
  return !isFilled() && !isEmpty();
}
bool isMoreThanHalfFilled() {
  return depth <= (maxDepth + minDepth) / 2;
}


long printStateAt = 0;
int state = STATE_UNKNOWN;

// Custom chars for LCD
byte barEmpty[8] = {
  B11111,
  B00000,
  B00000,
  B00000,
  B00000,
  B00000,
  B00000,
  B11111,
};
byte barOne[8] = {
  B11111,
  B10000,
  B10000,
  B10000,
  B10000,
  B10000,
  B10000,
  B11111,
};
byte barTwo[8] = {
  B11111,
  B11000,
  B11000,
  B11000,
  B11000,
  B11000,
  B11000,
  B11111,
};
byte barThree[8] = {
  B11111,
  B11100,
  B11100,
  B11100,
  B11100,
  B11100,
  B11100,
  B11111,
};
byte barFour[8] = {
  B11111,
  B11110,
  B11110,
  B11110,
  B11110,
  B11110,
  B11110,
  B11111,
};
byte warning_icon[8] = {
  B00000,
  B00100,
  B00100,
  B00100,
  B00100,
  B00000,
  B00100,
  B00000
};

// constants for ultrasonic sensor board IO pins
const int trigPin = 8, echoPin = 9;
// constants for Relay output
const int onPin = 12;
// constants for LCD IO pins
const int rs = 2, en = 3, d4 = 4, d5 = 5, d6 = 6, d7 = 7;
//Initailize libraries
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);
SimpleKalmanFilter timeFilter(1, 1, 0.01);

void setup() {
  Serial.begin(19200);
  // Set up IO pins for ultrasonic sensor board
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(onPin, OUTPUT);
  motorOff();
  // Set trigger pin for sensor to 0 (aka "do nothing yet")
  digitalWrite(trigPin, LOW);

  // set up the LCD's number of columns and rows:
  lcd.begin(16, 2);

  // Set custom chars 0-7
  lcd.createChar(0, barEmpty);
  lcd.createChar(1, barOne);
  lcd.createChar(2, barTwo);
  lcd.createChar(3, barThree);
  lcd.createChar(4, barFour);
  lcd.createChar(5, warning_icon);
  lcd.clear();
  // LCD part end

}


void loop() {
  // Do level scan with ultrasonic board

  // Start a scan - trigger pin must be high for at least 10us
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Get time echo pin is high using pulseIn, which returns value in microseconds
  // Default timeout of 1s is more than enough for 8 pulses, and we're not in a hurry
  timeHigh = pulseIn(echoPin, HIGH);

  if (timeHigh == 0) {
    // Oops! Timeout...
    showError();
  } else {

    currentLevel = round(timeHigh / 58);
    if (currentLevel > tankEmptyDepth) {
      // If level is lower than empty, show 0%
      // This is useful if you want to have "empty" be "still 10cm of liquid left in tank"
      currentLevel = tankEmptyDepth;
    } else if (currentLevel < tankFullDepth) {
      // If level is higher than full, show 100%
      // This is useful since "full" level may vary when tank is refilled
      currentLevel = tankFullDepth;
    }
    // Don't redraw screen if level is the same as last time
    if (currentLevel != lastLevel) {
      lastLevel = currentLevel;
      showCurrentLevel();
    }

  }

  // Delay 2s between scans
  delay(2000);
}

void showError() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(timeoutErrorTxt);
  lcd.setCursor(8, 1);
  lcd.print(char(5));
}

void showPartialBarChar(int val) {
  switch (val) {
    case 0:
      lcd.write(byte(0)); // barEmpty
      ++done;
      break;
    case 1:
      lcd.write(byte(1)); // one bar
      ++done;
      break;
    case 2:
      lcd.write(byte(2)); // two bars
      ++done;
      break;
    case 3:
      lcd.write(byte(3)); // three bars
      ++done;
      break;
    case 4:
      lcd.write(byte(4)); // four bars
      ++done;
      break;
  }
}

void showCurrentLevel() {

  float ratio = 1 - ((float)currentLevel - (float)tankFullDepth) / ((float)tankEmptyDepth - (float)tankFullDepth);
  ratio = abs(ratio);
  int textLevelInt = round(ratio * 100.0);
  int levelInt = round(ratio * 50.0);
  int fulls = 0;

  // Reset done
  done = 0;

  // Display progress bar based on levelInt
  lcd.clear();
  lcd.setCursor(0, 1);
  // Draw progress bar for XX%
  fulls = levelInt / 5;
  if (fulls == 0) {
    // First char on bar is a partial char with 0-4 vertical columns of pixels
    showPartialBarChar(levelInt);
  } else {
    for (int i = 0; i < fulls; ++i) {
      lcd.write(255); // full
      ++done;
    }
  }
  if (done < 10) {
    if (fulls > 0) {
      // Here we may have a partial char with 0-4 vertical columns of pixels
      showPartialBarChar(levelInt - (fulls * 5));
    }
    // Here we may have blank boxes left
    if (done < 10) {
      // We have empty boxes to draw
      for (int i = 0; i < 10 - done; ++i) {
        lcd.write(byte(0)); // barEmpty
      }
    }
  }

  // Lastly, print percentage:
  if (textLevelInt == 100) {
    lcd.setCursor(12, 1);
    lcd.print("100%");
  } else if (textLevelInt < 10) {
    lcd.setCursor(14, 1);
    lcd.print(textLevelInt);
    lcd.print("%");
  } else {
    lcd.setCursor(13, 1);
    lcd.print(textLevelInt);
    lcd.print("%");
  }
  // LCD Part Ends
  // Depth Calculation
  calculateDepth();

  if (millis() < 1000) {
    return;
  }

  // flow check
  switch (state) {
    case STATE_UNKNOWN:
      unknownState();
      break;
    case STATE_FILLING:
      fillingState();
      break;
    case STATE_EMPTYING:
      emptyingState();
      break;
    case STATE_NO_ELECTRICITY:
      noElectricityState();
      break;
    case STATE_WAITING_FOR_WATER:
      waitingForWaterState();
      break;
    case STATE_SENSOR_DISCONNECT:
      sensorDisconnectedState();
      break;
  }
  //motor automation
  if (state == STATE_FILLING) {
    motorOn();
  } else {
    motorOff();
  }

  if (state != STATE_SENSOR_DISCONNECT && depth == 0) {
    changeState(STATE_SENSOR_DISCONNECT);
    motorOff();
  }



  if (millis() > printStateAt) {

    if (state == STATE_FILLING) {
      Serial.println("motor On");
    } else {
      Serial.println("motorOff");
    }

    printStateAt = millis() + 1000;
    switch (state) {
      case STATE_UNKNOWN:
        Serial.println("STATE_UNKNOWN");
        break;
      case STATE_FILLING:
        Serial.println("STATE_FILLING");
        break;
      case STATE_EMPTYING:
        Serial.println("STATE_EMPTYING");
        break;
      case STATE_NO_ELECTRICITY:
        Serial.println("STATE_NO_ELECTRICITY");
        break;
      case STATE_WAITING_FOR_WATER:
        Serial.println("STATE_WAITING_FOR_WATER");
        break;
      case STATE_SENSOR_DISCONNECT:
        Serial.println("STATE_SENSOR_DISCONNECT");
        break;
    }

    depthCm = microsecondsToCentimeters(depth);

    Serial.print(depthCm);
    Serial.print("cm");
    Serial.println();
    Serial.println();
    delay(40);
  }
}
//Declairations
void changeState(long newState) {
  Serial.print("change_state: ");
  Serial.println(newState);

  stateSince = millis();
  state = newState;
  lastStateDepth = depth;
}
void unknownState() {
  if (isMoreThanHalfFilled()) {
    changeState(STATE_EMPTYING);
    motorOff();
  } else {
    changeState(STATE_FILLING);
    motorOn();
  }
}
void sensorDisconnectedState() {
  if (depth != 0) {
    changeState(STATE_UNKNOWN);
    return;
  }
  if (millis() > stateSince + 1000) {
    if (ledHigh) {
      digitalWrite(LED_BUILTIN, HIGH);
    } else {
      digitalWrite(LED_BUILTIN, LOW);
    }
    ledHigh = !ledHigh;
    stateSince = millis();
  }
}
void noElectricityState() {
  if (millis() >= stateSince + 15 * 60 * 1000) {
    changeState(STATE_FILLING);
    motorOn();
  }
}
void emptyingState() {
  if (isEmpty()) {
    changeState(STATE_FILLING);
    motorOn();
  }
}
void fillingState() {
  if (isFilled()) {
    changeState(STATE_EMPTYING);
    motorOff();
    return;
  }
  if (millis() >= stateSince + 30 * 1000) {
    // every 30s
    if (lastStateDepth - depth >= idealDepthIncreasePer30s / 2) {
      // actually filling
      changeState(STATE_FILLING);
    } else {
      changeState(STATE_NO_ELECTRICITY);
      motorOff();
      Serial.println("no water");
      lcd.setCursor(0, 0);
      lcd.print("No Water");
    }
  }
}
void waitingForWaterState() {
  if (millis() >= stateSince + 30 * 1000) {
    if (lastStateDepth - depth >= idealDepthIncreasePer30s / 2) {
      changeState(STATE_FILLING);
    } else if (millis() >= stateSince + 10 * 60 * 1000) {
      changeState(STATE_NO_ELECTRICITY);
      motorOff();
      Serial.println("no water");
      lcd.setCursor(0, 0);
      lcd.print("No Water");
    }
  }
}
// to be restructured
void calculateDepth() {
  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(trigPin, LOW);

  // The same pin is used to read the signal from the PING))): a HIGH pulse
  // whose duration is the time (in microseconds) from the sending of the ping
  // to the reception of its echo off of an object.
  durationRaw = pulseIn(echoPin, HIGH);
  depth = timeFilter.updateEstimate(durationRaw);
}
void motorOn() {
  digitalWrite(onPin, LOW);
}
void motorOff() {
  digitalWrite(onPin, HIGH);
}