#include <LiquidCrystal.h>
#include <SimpleKalmanFilter.h>
//lcd int
int rs = 2;
int en = 3;
int d4 = 4;
int d5 = 5;
int d6 = 6;
int d7 = 7;
//lcd int end
const int onPin = 12;
const int trigPin = 8;
const int echoPin = 9;
bool turnMotorOn = false;
bool turnMotorOff = false;
long turnMotorOnPulseEndAt = 0;
long turnMotorOffPulseEndAt = 0;

int LEDState = 0;
int LEDPin = 12;
int buttonPin = 10;
int buttonNew;
int buttonOld = 1;
int dt = 100;
SimpleKalmanFilter timeFilter(1, 1, 0.01);
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);
//create byte
uint8_t gauge_empty[8] =  {B11111, B00000, B00000, B00000, B00000, B00000, B00000, B11111};    // empty middle piece
uint8_t gauge_fill_1[8] = {B11111, B10000, B10000, B10000, B10000, B10000, B10000, B11111};    // filled gauge - 1 column
uint8_t gauge_fill_2[8] = {B11111, B11000, B11000, B11000, B11000, B11000, B11000, B11111};    // filled gauge - 2 columns
uint8_t gauge_fill_3[8] = {B11111, B11100, B11100, B11100, B11100, B11100, B11100, B11111};    // filled gauge - 3 columns
uint8_t gauge_fill_4[8] = {B11111, B11110, B11110, B11110, B11110, B11110, B11110, B11111};    // filled gauge - 4 columns
uint8_t gauge_fill_5[8] = {B11111, B11111, B11111, B11111, B11111, B11111, B11111, B11111};    // filled gauge - 5 columns
uint8_t gauge_left[8] =   {B11111, B10000, B10000, B10000, B10000, B10000, B10000, B11111};    // left part of gauge - empty
uint8_t gauge_right[8] =  {B11111, B00001, B00001, B00001, B00001, B00001, B00001, B11111};    // right part of gauge - empty
uint8_t warning_icon[8] = {B00000, B00100, B00100, B00100, B00100, B00000, B00100, B00000};     // warning icon - just because we still have one custom character left
uint8_t gauge_left_dynamic[8];   // left part of gauge dynamic - will be set in the loop function
uint8_t gauge_right_dynamic[8];  // right part of gauge dynamic - will be set in the loop function
uint8_t gauge_mask_left[8] = {B01111, B11111, B11111, B11111, B11111, B11111, B11111, B01111};  // mask for rounded corners for leftmost character
uint8_t gauge_mask_right[8] = {B11110, B11111, B11111, B11111, B11111, B11111, B11111, B11110}; // mask for rounded corners for rightmost character

char buffer[10];         // helper buffer to store C-style strings (generated with sprintf function)
int move_offset = 0;     // used to shift bits for the custom characters

const int gauge_size_chars = 16;       // width of the gauge in number of characters
char gauge_string[gauge_size_chars + 1]; // string that will include all the gauge character to be printed
//byte end
// micros
long depth, durationRaw;
long depthCm;
long refreshSerial = 0;

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

long stateStartAt = 0;
int state = STATE_UNKNOWN;

// all are in duration (micros)
int tankDepth = 104 * 58;
int maxDepth = 85 * 58;
int minDepth = 20 * 58;

bool isFilled() {
  return depth <= minDepth;
}

bool isEmpty() {
  return depth > maxDepth;
}

bool isPartial() {
  return !isFilled() && !isEmpty();
}

long printStateAt = 0;
void motorOn() {
  digitalWrite(onPin, LOW);
}

void motorOff() {
  digitalWrite(onPin, HIGH);
}

void setup() {
  //progress bar
  //lcd.init();                       // initialize the 16x2 lcd module
  lcd.createChar(7, gauge_empty);   // middle empty gauge
  lcd.createChar(1, gauge_fill_1);  // filled gauge - 1 column
  lcd.createChar(2, gauge_fill_2);  // filled gauge - 2 columns
  lcd.createChar(3, gauge_fill_3);  // filled gauge - 3 columns
  lcd.createChar(4, gauge_fill_4);  // filled gauge - 4 columns
  lcd.createChar(0, warning_icon); // warning icon - just because we have one more custom character that we could use
  //lcd.backlight();
  //progress bar end
  Serial.begin(9600);
  pinMode(LEDPin, OUTPUT);
  pinMode(buttonPin, INPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(onPin, OUTPUT);
  motorOff();
  lcd.begin(16, 2);
}

void loop() {

  //progress bar start
  {
    float units_per_pixel = (gauge_size_chars * 5.0) / 100.0;    //  every character is 5px wide, we want to count from 0-100
    int value_in_pixels = round(digitalRead(echoPin) * units_per_pixel);    // cpu_gauge value converted to pixel width (change cpu_gauge to depth)

    int tip_position = 0;      // 0= not set, 1=tip in first char, 2=tip in middle, 3=tip in last char

    if (value_in_pixels < 5) {
      tip_position = 1; // tip is inside the first character
    }
    else if (value_in_pixels > gauge_size_chars * 5.0 - 5) {
      tip_position = 3; // tip is inside the last character
    }
    else {
      tip_position = 2; // tip is somewhere in the middle
    }

    move_offset = 4 - ((value_in_pixels - 1) % 5);    // value for offseting the pixels for the smooth filling

    for (int i = 0; i < 8; i++) { // dynamically create left part of the gauge
      if (tip_position == 1) {
        gauge_left_dynamic[i] = (gauge_fill_5[i] << move_offset) | gauge_left[i]; // tip on the first character
      }
      else {
        gauge_left_dynamic[i] = gauge_fill_5[i]; // tip not on the first character
      }

      gauge_left_dynamic[i] = gauge_left_dynamic[i] & gauge_mask_left[i];                                 // apply mask for rounded corners
    }

    for (int i = 0; i < 8; i++) { // dynamically create right part of the gauge
      if (tip_position == 3) {
        gauge_right_dynamic[i] = (gauge_fill_5[i] << move_offset) | gauge_right[i]; // tip on the last character
      }
      else {
        gauge_right_dynamic[i] = gauge_right[i]; // tip not on the last character
      }

      gauge_right_dynamic[i] = gauge_right_dynamic[i] & gauge_mask_right[i];                                // apply mask for rounded corners
    }

    lcd.createChar(5, gauge_left_dynamic);     // create custom character for the left part of the gauge
    lcd.createChar(6, gauge_right_dynamic);    // create custom character for the right part of the gauge

    for (int i = 0; i < gauge_size_chars; i++) { // set all the characters for the gauge
      if (i == 0) {
        gauge_string[i] = byte(5); // first character = custom left piece
      }
      else if (i == gauge_size_chars - 1) {
        gauge_string[i] = byte(6); // last character = custom right piece
      }
      else {                                                        // character in the middle, could be empty, tip or fill
        if (value_in_pixels <= i * 5) {
          gauge_string[i] = byte(7); // empty character
        }
        else if (value_in_pixels > i * 5 && value_in_pixels < (i + 1) * 5) {
          gauge_string[i] = byte(5 - move_offset); // tip
        }
        else {
          gauge_string[i] = byte(255); // filled character
        }
      }
    }

    // gauge drawing
   // lcd.setCursor(0, 0);                        // move cursor to top left
    //sprintf(buffer, "CPU:%3d%% ", cpu_gauge);    // set a string as CPU: XX%, with the number always taking at least 3 character
   // lcd.print(buffer);                          // print the string on the display
    //lcd.print(byte(0));                         // print warning character

    lcd.setCursor(0, 1);             // move the cursor to the next line
    lcd.print(gauge_string);         // display the gauge



    // increase the CPU value, set between 0-100
    //depth = depthCm + 1 ;
    //if (depth > 100) {
      //depth = 0;
//    }

    //delay(500);  // wait for a while - 100ms = update the screen 10x in a second
    //progress bar end
    //lcd.setCursor(0,0);
    //lcd.print("Motor Controller");
    buttonNew = digitalRead(buttonPin);
    if (buttonOld == 0 && buttonNew == 1) {
      if (LEDState == 0) {
        digitalWrite(LEDPin, HIGH);
        LEDState = 1;
        Serial.println("LED Off");
        lcd.setCursor(0, 1);
        lcd.print(char(4));
        delay(200);
      }
      else {
        digitalWrite(LEDPin, LOW);
        LEDState = 0;
        Serial.println("LED On");
      }
    }
    
    buttonOld = buttonNew;
    delay(dt);
  

  }

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