#include "Arduino.h"
#include <stdio.h>
#include <string.h>
#include <cstring>

//Sensor Pins
#define TRIG_PIN 32  		//Transmit 	– Transmit signal for the sensor transmit readings
#define ECHO_PIN 35			//Recieve 	–	Recieve the transmission from sensor

//SIM800L Module Pins
#define RX_PIN 16				//Recieve		–	Recieve UART transmission from SIM module
#define TX_PIN 17  			//Transmit	– Transmit UART instructions to SIM module
#define	SLEEP_MODE_PIN 4//Transmit	– Transmit for the SIM module to go to sleep mode
#define RESTART_PIN 5		//Trasmit		– Transmit for the SIM module to do a hard restart

uint8_t stage = 0;//0 – less than 50% full, 1 – +50% full, 2 – +80% full
bool simModuleOn = false;

//Functions
uint16_t readDistanceCM();
void 	startupSimModule();
void 	restartSimModule();
void 	sleepSimModule();
void 	wakeSimModule();
bool 	sendSMS(String message);
bool sendATCommand(String command, String expectedResponse);
bool 	validateModuleInit();
bool 	validateNetworkRegistration();
bool 	validateSimCard();
bool 	setSMSMode();

//UART
//Comunication protocol to comunicate with the SIM module
#include <SoftwareSerial.h>
EspSoftwareSerial::UART mySerial;

//Sleep Mode Variables
#define SLEEP_DURATION 30 //seconds.  20 min = 1200

//SMS Message Variables
#define PHONE_NUMBER "+37000000000"
#define UNIT_ID	1	//Could be some type of trash code
#define ADDRESS "Kaunas Baguette g. 23"

void setup() {
  //Initialize Pins
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
	pinMode(SLEEP_MODE_PIN, OUTPUT);
	digitalWrite(SLEEP_MODE_PIN, LOW);
	pinMode(RESTART_PIN, OUTPUT);
	digitalWrite(RESTART_PIN, HIGH);
	
	//Sleep mode configuration
	esp_sleep_enable_timer_wakeup(SLEEP_DURATION * 1000000UL); 


	//Start Console/UART communication
	Serial.begin(9600);
	Serial.println("Hello, ESP32!");
	mySerial.begin(9600, EspSoftwareSerial::SWSERIAL_8N1, RX_PIN, TX_PIN);
	
	delay(3000);
	//Startup SIM Module
	startupSimModule();
}

void loop() {
	bool SMS = false;
	//Get readings from HC–SR04 Sensor
  uint16_t cm = readDistanceCM();
	uint8_t percentage = ((400 - cm) / 4);
	Serial.print("Full: ");
	Serial.print(percentage); 
	Serial.println("%");
	//Check if it needs to send a message
	if(simModuleOn)
	{
		if(cm <=80)
		{
			if(stage != 2)
			{
				stage = 2;
				SMS = true;
			}
		}
		else if(cm <= 200)
		{
			if(stage == 0)
			{
				stage = 1;
				SMS = true;
			}
		}
		else if(cm > 300)
		{
			if(stage != 0)
			{
				stage = 0;
				Serial.println("Container Emptied.");
			}
		}
		//Send SMS
		if(SMS)
		{
			String message = "Prietaisas: '" +  String(UNIT_ID);
			message = message + "' Adresas: '" + ADDRESS ;
			message = message + "' Užpildyta: '" + String() + "'";
			if(sendSMS(message))
			{
				Serial.println("SMS was sent successfully.");
			}
			else
			{
				stage = stage - 1;
				Serial.println("SMS could not be sent.");
				simModuleOn = false;
			}
		}
		else
		{
			if(stage == 0)
			{
				Serial.println("SMS was not needed.");
			}
			else
			{
				Serial.println("SMS was already sent.");
			}
		}
		// Put SIM Module to sleep
		sleepSimModule();
	}
  // Put ESP32 to sleep
  //esp_deep_sleep_start(); Wokwi doesn't simulate sleep well, wokwi workaround: delay()
	Serial.println("Entering Sleep Mode.");
	delay(SLEEP_DURATION * 1000);
	Serial.println("Exiting Sleep Mode.");
	// Wake SIM module up
	if(simModuleOn)
	{
		wakeSimModule();
	}
	else
	{
		restartSimModule();
	}
	
}

//	Function: readDistanceCM()
//	uint16_t was selected	because int8_t is too small(-128 – 127) as it needs to go up to 400, and unsinged because the minimum is 0 
//
//	Sends signal to the sensor to transmit data 
//	Recieves the transimssion and converts it into centimeters and rounds to whole numbers
//	Returns centimeters
uint16_t readDistanceCM() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  return round(pulseIn(ECHO_PIN, HIGH) * 0.034 / 2);
}

//	Function:	startupSimModule()
//
//	Checks if module is initialized and everything is connected
//	if yes, ends function and does nothing
//	if no, turns off SIM Module functionality
void startupSimModule()
{
	Serial.println("Starting up SIM Module...");
	for(uint8_t i = 0; i < 3; i++)
	{
		if(validateModuleInit())
		{
			if(!(validateNetworkRegistration() && validateSimCard() && setSMSMode()))
			{
				delay(1000);
			}
			else
			{
				simModuleOn = true;
				return;//All checks pass, ends function
			}
		}
		delay(2000);
		Serial.println("Rechecking...");
	}
	simModuleOn = false;
	return;
}

//	Function:	restartSimModule()
//
//	Does a hard restart of the SIM Module and calls startupSimModule() function
void restartSimModule()
{
	Serial.println("Restarting SIM Module...");
	digitalWrite(RESTART_PIN, HIGH);
	delay(100);
	digitalWrite(RESTART_PIN, LOW);
	Serial.println("Restarted.");
	delay(1000);
	startupSimModule();
	return;
}

//	Function:	sleepSimModule()
//
//	Tries to put the module to sleep mode
//	If it succeeds, ends function 
//	if not, calls	restartSimModule() function and tries to put it to sleep again
void sleepSimModule()
{
	for(uint8_t i = 0; i < 3; i++)
	{
		Serial.println("Putting Module to sleep mode...");
		if(sendATCommand("AT+CSCLK=1", "OK"))
		{
			Serial.println("Module in sleep mode.");
			return;
		}
		else
		{
			Serial.println("Failed to enter sleep mode.");
		}
	}
	Serial.println("Sleep mode could not be turned on.");
	restartSimModule();
	sleepSimModule();
	return;
}

//	Function:	wakeSimModule()
//
//	Tries to put the module out of sleep mode
//	If it succeeds, ends function 
//	if not, calls	restartSimModule() function to wake it
void wakeSimModule()
{
	Serial.println("Waking Module from sleep mode...");
	digitalWrite(SLEEP_MODE_PIN, HIGH);
	sendATCommand("AT", "OK");
	if(sendATCommand("AT+CSCLK=0", "OK"))
	{
		Serial.println("Waked from sleep mode.");
		digitalWrite(SLEEP_MODE_PIN, LOW);
		return;
	}
	
	Serial.println("Failed to exit sleep mode.");
	digitalWrite(SLEEP_MODE_PIN, LOW);
	restartSimModule();
	delay(100);
	return;
}

//	Function: sendSMS(String message) 
//	Function Argument:	String message	–	String that contains the text message that will be sent
//	
//	Sends a command to the Module to send an SMS
//	returns the status of the SMS
bool sendSMS(String message) 
{
		String command = "AT+CMGS=\\" + String(PHONE_NUMBER) + "\\";
    if(!sendATCommand(command, ">"))
		{
			return 0;
		}
    mySerial.print(message);
    mySerial.write(26);
    return sendATCommand("AT+CMGR=0", "OK");
}

//	Function: sendATCommand(String command, String expectedResponse)
//	Function Argument: String command – String that contains the command to the SIM Module
//	Function Argument: String expectedResponse – String that contains the expected response to the command
//
//	Sends the command with UART
//	Recieves the response
//	Compares if the expected response matches the actual response
//	returns the comparision between the expected response and the actual response result
bool sendATCommand(String command, String expectedResponse) 
{
	command = command + '\n';
	mySerial.print(command);

	// Read the entire response into a String
	String response = mySerial.readStringUntil(10);

	//Compare expected with real response
	bool responseCompare = strcmp(expectedResponse.c_str(), response.c_str()) == 0;
	if(!responseCompare)
	{
		Serial.print("The Expected Response: ");
		for(uint8_t i = 0; i < sizeof(expectedResponse.c_str())/sizeof(char); i++)
		{
			Serial.print((uint8_t)expectedResponse.c_str()[i]);
			Serial.print(" ");
		}
		Serial.println("");
		Serial.print("The Actual Response  : ");
		for(uint8_t i = 0; i < sizeof(response.c_str())/sizeof(char); i++)
		{
			Serial.print((uint8_t)response.c_str()[i]);
			Serial.print(" ");
		}
		Serial.println("");
	}

	return responseCompare;
}

//	Function: validateModuleInit() 
//
//	Checks if the module is recieving commands
//	Returns the status of the module initialization
bool validateModuleInit() 
{
    Serial.println("Initializing SIM800L...");
		bool OK = sendATCommand("AT", "OK");
    if (OK) {
        Serial.println("SIM800L initialized successfully.");
        return true;
    } else {
        Serial.println("Failed to initialize SIM800L.");
				Serial.println("Check if the module is connected correctly.");
        return false;
    }
}

//	Function: validateNetworkRegistration() 
//
//	Checks if the module has mobile service
//	returns the status of the mobile service connection
bool validateNetworkRegistration() 
{
    Serial.println("Checking network registration...");
		bool OK = sendATCommand("AT+CREG?", "+CREG: 0");
    if (OK) {
        Serial.println("Successfully registered to the network.");
        return true;
    } else {
        Serial.println("Failed to register to the network.");
        return false;
    }
}

//	Function: validateSimCard() 
//
//	Checks if the SIM card is valid or even inserted
//	returns the status of the SIM card
bool validateSimCard() 
{
    Serial.println("Checking SIM card...");
		bool OK = sendATCommand("AT+CCID", "0");
    if (OK) {
        Serial.println("Valid SIM card detected.");
        return true;
    } else {
        Serial.println("Invalid SIM card or no SIM present.");
        return false;
    }
}

//	Function: setSMSMode()
//
//	Sets the module to Text Mode SMS
//	returns the status of the configuration change
bool setSMSMode() {
	Serial.println("Setting up SMS...");
  return sendATCommand("AT+CMGF=1", "OK");
}