// import a library for an LCD
#include <LiquidCrystal.h>

// functions declaration
void diodeToggle();
float readDistanceCM();
void writeLCDMsg();
void writeRangeDistance();
void resetDistance();
void lcdRowReset(int colNr = 0, int rowNr = 0, int endPosition = 15);
void createSpecialCharacters();

// LCD setup
const int RS = 11,
			E = 12,
			D4 = 6,
			D5 = 5,
			D6 = 4,
			D7 = 3;
LiquidCrystal lcd(RS,E,D4,D5,D6,D7);

const int LCD_COLS = 16;
const int LCD_ROWS = 2;
const int LCD_2_ROW_START_COL = 2;


// pins declarations
const int RED_LED_PIN = 8;
const int YELLOW_LED_PIN = 7;
const int GREEN_LED_PIN = 2;

const int SENSOR_TRIG_PIN = 10;
const int SENSOR_ECHO_PIN = 9;


// arrays with pin numbers, appropriate diode colors & led states
const int LED_PIN_NRS[] = {RED_LED_PIN, YELLOW_LED_PIN, GREEN_LED_PIN};
//const int BTN_PIN_NRS[] = {redBtnPin, yellowBtnPin, greenBtnPin};
const char *const LED_PIN_COLORS[] = {"red", "yellow", "green"};
const char *const LED_PIN_COLORS_PL[] = {"czerwony", "żółty", "zielony"};
int ledStates[] = {0, 0, 0};

// length counter for the above array
const int AMOUNT_OF_PINS_USED = (sizeof(LED_PIN_NRS) / sizeof(LED_PIN_NRS[0]));

// arrays for distance sensor
const char *const DISTANCE_STATES[] = {"Bardzo blisko", "Blisko", "Daleko", "Bardzo daleko"};
const int DISTANCE_RANGES[] = {100, 200, 300, 400};
const int AMOUNT_OF_DISTANCE_RANGES = (sizeof(DISTANCE_RANGES) / sizeof(DISTANCE_RANGES[0]));


// diodes counter
int x = 0;

int currentDistance = 0;


// time counter initial values
unsigned long previousMillis = 0;
const long INTERVAL = 1000;
unsigned long currentMillis = 0;


const byte a_[8] = { //ą
  B00000,
	B01110,
  B00001,
  B01111,
  B10001,
  B01111,
  B00010,
  B00001,
};
const byte c_[8] = { //ć
  B00010,
  B00100,
  B01110,
  B10000,
  B10000,
  B10001,
  B01110,
  B00000,
};
const byte e_[8] = { //ę
  B00000,
  B01110,
  B10001,
  B11111,
  B10000,
  B01110,
  B00100,
  B00010,
};
const byte l_[8] = { //ł
  B01100,
  B00100,
  B00100,
  B00110,
  B01100,
  B00100,
  B01110,
  B00000,
};
const byte n_[8] = { //ń
	B00010,
  B00100,
  B10110,
  B11001,
  B10001,
  B10001,
  B10001,
  B00000,
};
const byte o_[8] = { //ó
	B00010,
  B00100,
  B01110,
  B10001,
  B10001,
  B10001,
  B01110,
  B00000,
};
const byte s_[8] = { //ś
	B00010,
  B00100,
  B01110,
  B10000,
  B01110,
  B00001,
  B11110,
  B00000,
};
const byte z__[8] = { //ź
  B00010,
  B00100,
  B11111,
  B00010,
  B00100,
  B01000,
  B11111,
  B00000,
};
const byte z_[8] = { //ż
  B00100,
  B00000,
  B11111,
  B00010,
  B00100,
  B01000,
  B11111,
  B00000,
};
const byte heartSymbol[8] = {
  B00000,
  B01010,
  B11111,
  B11111,
  B01110,
  B00100,
  B00000,
  B00000
};
const byte resetCharacter[8] = {
  B00000,B00000,B00000,B000000,B00000,B00000,B00000,B00000
};



void setup() {
  // set up a terminal
  Serial.begin(9600);
	createSpecialCharacters();
	// lcd printing init
	lcd.begin(16,2);
	// set cursor at the specific point (col, row)
  lcd.setCursor(15,0);
  lcd.write(byte(3));
	// declare using pins
	for (int i = 0; i < AMOUNT_OF_PINS_USED; i++) {
		pinMode(LED_PIN_NRS[i], OUTPUT);
  }
  // set up sensor's pins
  pinMode(SENSOR_TRIG_PIN, OUTPUT);
  pinMode(SENSOR_ECHO_PIN, INPUT);
  //writeLCDMsg();
}


// main loop for reading sensor actions
void loop() {
  currentMillis = millis();

  if(currentMillis-previousMillis >= INTERVAL) {
    currentDistance = getDistanceCM();
    writeLCDMsg();
    diodeToggle();
    previousMillis = currentMillis;
  }
}


// trigerring the sensor for getting distance
float getDistanceCM() {
  digitalWrite(SENSOR_TRIG_PIN, LOW);
  digitalWrite(SENSOR_TRIG_PIN, HIGH);
  digitalWrite(SENSOR_TRIG_PIN, LOW);

  int duration = pulseIn(SENSOR_ECHO_PIN, HIGH);
  Serial.println(duration);
  return duration * 0.0340/2;
}



// switch the light of a diode
// write status on the LCD
void diodeToggle() {
  int ledState = 0;

	for (int i = 0; i < AMOUNT_OF_PINS_USED; i++) {
    // check if the LED should light up
    ledState = (x == i);
    digitalWrite(LED_PIN_NRS[i], ledState);
    Serial.println(String(LED_PIN_COLORS_PL[i]) + ":   " + String(ledStates[i]));
    ledStates[i] = ledState;
  }
}



// set cursor below the initially written "Light status "
// and write status of the specific diode
void writeLCDMsg() {
  lcdRowReset(1, 1);
  Serial.print("\n" + String(currentDistance) + "cm");
	lcd.setCursor(1, 0);
  lcdRowReset(1, 0, 5);
  lcd.print(String(currentDistance) + "cm");
  writeDistanceRange();
}


void writeDistanceRange() {
	lcd.setCursor(1, 1);
  for(int i = 0; i < AMOUNT_OF_DISTANCE_RANGES; i++) {

    if(currentDistance <= DISTANCE_RANGES[i]) {

      // it is 4 distance ranges and 3 diodes
      // -> for the last 2 ranges the green diode works
      // so we have to assign a max value to the x variable
      // which is AMOUNT_OF_PINS_USED-1
      // amount equals 3 and we start counting from 0, so maximum is 2
      if ( i >= AMOUNT_OF_PINS_USED ) {
        x = AMOUNT_OF_PINS_USED-1;
      } else {
        x = i;
      }

      // print in terminal
      Serial.println("   to   " + String(DISTANCE_STATES[i]));
      // print in lcd
      lcd.print(DISTANCE_STATES[i]);
      break;
    }

  }
}


// debugging: reset LCD, clear previously displayed text
void lcdRowReset(int colNr=0, int rowNr=0, int endPosition=15) {
	lcd.setCursor(colNr, rowNr);
  for(int i = 0; i <= endPosition; i++) {
	  lcd.print(" ");
  }
	lcd.setCursor(colNr, rowNr);
}


void createSpecialCharacters() {
	// maximum is 0 - 7
  // reset character to avoid problem
  // with displaying letters
  // after e.g. changing their index nr
	//lcd.createChar(0, resetCharacter);
	//lcd.createChar(0, a_);

	//lcd.createChar(, resetCharacter);
	//lcd.createChar(, c_);

	//lcd.createChar(, resetCharacter);
	//lcd.createChar(, e_);

	//lcd.createChar(1, resetCharacter);
	//lcd.createChar(1, l_);

	//lcd.createChar(, resetCharacter);
	//lcd.createChar(, n_);

	//lcd.createChar(2, resetCharacter);
	//lcd.createChar(2, o_);

	//lcd.createChar(, resetCharacter);
	//lcd.createChar(, s_);

	//lcd.createChar(4, resetCharacter);
	//lcd.createChar(4, z__);

	//lcd.createChar(2, resetCharacter);
	//lcd.createChar(2, z_);

	lcd.createChar(3, resetCharacter);
	lcd.createChar(3, heartSymbol);
  
  //lcd.write(byte(0));
  //lcd.write(byte(1));
  //lcd.write(byte(2));
  //lcd.write(byte(3));
  //lcd.write(byte(4));
  //lcd.write(byte(5));
  //lcd.write(byte(6));
  //lcd.write(byte(7));
}