#include "StateMachineLib.h"
#include "AsyncTaskLib.h"
#include <LiquidCrystal.h>
#include <Keypad.h>
#include "DHT.h"
#define DHTPIN A1
#define DHTTYPE DHT22//DHT11
#include <AverageValue.h>
#define PHOTOCELL_PIN A0
#define BUZZER 6
//lcd config
LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
short int luz = 0.0;

//RGB config constants
const int redPin = 7; 
const int greenPin = 8;
const int bluePin = 9;

//PHOTO constants
const float GAMMA = 0.7;
const float RL10 = 50;

//RGB functions
void color (unsigned char red, unsigned char green, unsigned char blue) // the color generating function
{
analogWrite(redPin, red);
analogWrite(bluePin, blue);
analogWrite(greenPin, green);
}

//Init variables for password
char password[] = {'1','2','3','4'};
int passwordVerify [4];
int numLetter = 0;
int gblCountMistakes = 0;
int countCorrect = 0;

//keypad config
const byte ROWS = 4;
const byte COLS = 4;
char keys[ROWS][COLS] = {
  {'1','2','3','A'},
  {'4','5','6','B'},
  {'7','8','9','C'},
  {'*','0','#','D'}
};
byte rowPins[ROWS] = {24, 26, 28, 30}; 
byte colPins[COLS] = {32, 34, 36, 38};
Keypad keypad = Keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS );

// Create new StateMachine
StateMachine stateMachine(5, 8);
int gblPassing = 0;

// State Alias
enum State
{
	StateInit = 0,
	StateBlocked = 1,
	StateMonitorTH = 2,
	StateAlarm = 3,
  StateMonitorLight = 4,
};

// Input Alias
enum Input
{
	InInit = 0,
	InBlocked = 1,
	InMonitorTH = 2,
	InAlarm = 3,
  InLight = 4,
	signTime = 5
};

// Stores last user input
Input input;

// Tasks
AsyncTask time(5000, false, []() { fnTime(); });
AsyncTask initTask(50, true, []() { fnInit(); });
AsyncTask monitorTHTask(50, true, []() { fnMonitorTH(); });
AsyncTask monitorLuzTask(50, true, []() { fnMonitorLuz(); });

void fnTime()
{
	input = signTime;
	gblPassing = 0;
}

void inputInit()
{
	initTask.Start();
}

void inputMonitorTH()
{
	monitorTHTask.Start();
}

void inputMonitorLuz()
{
	monitorLuzTask.Start();
}

void outputInit()
{
	initTask.Stop();
}

void outputMonitorTH()
{
	monitorTHTask.Stop();
}

void outputMonitorLuz()
{
	monitorLuzTask.Stop();
}

// Setup the State Machine
void setupStateMachine()
{
	// Add transitions
	stateMachine.AddTransition(StateInit, StateBlocked, []() { return input == InBlocked; });
  stateMachine.AddTransition(StateInit, StateMonitorTH, []() { return input == InMonitorTH; });

	stateMachine.AddTransition(StateBlocked, StateInit, []() { return input == signTime; });

	stateMachine.AddTransition(StateMonitorTH, StateAlarm, []() { return input == InAlarm; });
	stateMachine.AddTransition(StateMonitorTH, StateMonitorLight, []() { return input == signTime; });

  stateMachine.AddTransition(StateAlarm, StateMonitorTH, []() { return input == InMonitorTH; });

  stateMachine.AddTransition(StateMonitorLight, StateAlarm, []() { return input == InAlarm; });
	stateMachine.AddTransition(StateMonitorLight, StateMonitorTH, []() { return input == InMonitorTH; }); //cambiar sign

	// Add actions
	stateMachine.SetOnEntering(StateInit, inputInit);
	stateMachine.SetOnEntering(StateBlocked, inputBlocked);
	stateMachine.SetOnEntering(StateMonitorTH, inputMonitorTH);
	stateMachine.SetOnEntering(StateAlarm, inputAlarma);
  stateMachine.SetOnEntering(StateMonitorLight, inputMonitorLuz);

	stateMachine.SetOnLeaving(StateInit, []() {outputInit(); });
	stateMachine.SetOnLeaving(StateMonitorTH, []() {outputMonitorTH(); });
	stateMachine.SetOnLeaving(StateAlarm, []() {Serial.println("Leaving D"); });
  stateMachine.SetOnLeaving(StateMonitorLight, []() {outputMonitorLuz(); });
}

void setup() 
{
	//setup
	lcd.begin(16, 2);
	//Setup for RGB
	pinMode(redPin, OUTPUT); 
  pinMode(greenPin, OUTPUT);
  pinMode(bluePin, OUTPUT);
	pinMode(BUZZER, OUTPUT); 

	//Setup for State machine
	Serial.begin(9600);
	setupStateMachine();	

	// Initial state
	stateMachine.SetState(StateInit, false, true);
}

void loop() 
{
	monitorLuzTask.Update();
	time.Update();
	initTask.Update();
	monitorTHTask.Update();
	stateMachine.Update();
	
}

//Functions for password
void accessGranted(){
	lcd.clear();
  color(0, 255, 0);
  lcd.print("Correcto");
  delay(800);
	lcd.clear();
	color(0, 0, 0);
	input = InMonitorTH;
}
void incorrectPassword(){
	lcd.clear();
  if(gblCountMistakes < 2){
    color(0, 0, 255);
    lcd.print("Incorrecto");
    delay(500);
    gblCountMistakes++;
		lcd.clear();
		lcd.print("Password: ");
  }
  else{
		countCorrect = -1;
		input = InBlocked;
  }
}

// Auxiliar output functions that show the state debug
void fnInit()
{
	lcd.clear();
	numLetter = 0;
	countCorrect = 0;
	gblCountMistakes = 0;
	lcd.print("Password: ");
	do{
		color(0, 0, 0);
		char key = keypad.getKey();
		if(key){
		lcd.print('*');
		passwordVerify[numLetter] = key;
		numLetter++;
		color(255, 255, 255);
		delay(100);
		}
		if(numLetter == 4){
			countCorrect = 0;
			for(int i=0; i<numLetter;i++){
				if(password[i] == passwordVerify[i]){
					countCorrect++;
				}
			}
			if(countCorrect == numLetter){
				accessGranted();
				numLetter = 0;
			}
			else{
				incorrectPassword();
				numLetter = 0;
			}
		}
		
	}while((countCorrect != -1) && (countCorrect != 4));
}

void inputBlocked()
{
	color(255, 0, 0);
	lcd.print("Bloqueado");
	time.SetIntervalMillis(5000);
	time.Start();
}

DHT dht(DHTPIN, DHTTYPE);
const long MAX_VALUES_NUM = 5;
AverageValue<long> averageValueT(MAX_VALUES_NUM);
AverageValue<long> averageValueH(MAX_VALUES_NUM);
AverageValue<long> cleanAverageValue(MAX_VALUES_NUM);

void fnMonitorTH()
{
	lcd.clear();
	countCorrect = 0;
	averageValueT = cleanAverageValue;
	averageValueH = cleanAverageValue;
	numLetter = 0;
	dht.begin();
	while(gblPassing == 0)
	{
		float h = dht.readHumidity();
    float t = dht.readTemperature();
    float f = dht.readTemperature(true);

    if (isnan(h) || isnan(t) || isnan(f)) {
        Serial.println(F("Failed to read from DHT sensor!"));
        return;
    }

    float hif = dht.computeHeatIndex(f, h);
    float hic = dht.computeHeatIndex(t, h, false);

		lcd.setCursor(0, 0);
    lcd.print("Humidity: ");
    lcd.print(h);
    lcd.print("%");

    lcd.setCursor(0, 1);
    lcd.print("Temp: ");
    lcd.print(t);
    lcd.print("C");
		delay(500);
		averageValueT.push(t);
		averageValueH.push(h);
		numLetter++;
		if(numLetter >= 5)
		{
			if(averageValueT.average() > 30 && averageValueH.average() > 70)
			{
				input = InAlarm;
				countCorrect++;
				break;
			}
			else
			{
				countCorrect++;
				time.SetIntervalMillis(5000);
				time.Start();
				gblPassing++;
			}
			numLetter = 0;
		}
	}
}

void inputAlarma() {
    lcd.clear();
    color(255, 0, 0);
    lcd.print("Alarma");
    tone(BUZZER, 1000, 5000);
    delay(6000); 
    noTone(BUZZER);
		input = InMonitorTH;
}

AverageValue<long> averageValueLuz(MAX_VALUES_NUM);
void fnMonitorLuz()
{   
    int analogValue = analogRead(PHOTOCELL_PIN);
    float voltage = analogValue / 1024.0 * 5.0;
    float resistance = 2000.0 * voltage / (1.0 - voltage / 5.0);
    float luz = pow(RL10 * 1e3 * pow(10, GAMMA) / resistance, (1.0 / GAMMA));
		lcd.print("Luz: ");
		lcd.print(luz);
		delay(1000); 

    if (luz < 20.0) {
        input = InAlarm;
    } else {
        delay(3000); // Delay for 3 seconds
        input = InMonitorTH;
    }
}