#include "StateMachineLib.h"

// 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,
};

// Create new StateMachine
StateMachine stateMachine(5, 8);

// Stores last user input
Input input;

// 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 == InInit; });

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

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

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

	// Add actions
	stateMachine.SetOnEntering(StateInit, outputA);
	stateMachine.SetOnEntering(StateBlocked, outputB);
	stateMachine.SetOnEntering(StateMonitorTH, outputC);
	stateMachine.SetOnEntering(StateAlarm, outputD);
  stateMachine.SetOnEntering(StateMonitorLight, outputE);

	stateMachine.SetOnLeaving(StateInit, []() {Serial.println("Leaving Init"); });
	stateMachine.SetOnLeaving(StateBlocked, []() {Serial.println("Leaving B"); });
	stateMachine.SetOnLeaving(StateMonitorTH, []() {Serial.println("Leaving C"); });
	stateMachine.SetOnLeaving(StateAlarm, []() {Serial.println("Leaving D"); });
  stateMachine.SetOnLeaving(StateMonitorLight, []() {Serial.println("Leaving E"); });
}

void setup() 
{
	Serial.begin(9600);

	Serial.println("Starting State Machine...");
	setupStateMachine();	
	Serial.println("Start Machine Started");

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

void loop() 
{
	// Read user input
	input = static_cast<Input>(readInput());

	// Update State Machine
	stateMachine.Update();
}

// Auxiliar function that reads the user input
int readInput()
{
  // hacer que cada vez que se ejecute no cambie la variable
	Input currentInput = Input::InInit;
	if (Serial.available())
	{
		char incomingChar = Serial.read();

		switch (incomingChar)
		{
			case '0': currentInput = Input::InInit; 	break;
			case '1': currentInput = Input::InBlocked; break;
			case '2': currentInput = Input::InMonitorTH; break;
      case '3': currentInput = Input::InAlarm; break;
      case '4': currentInput = Input::InLight; break;
			default: break;
		}
	}

	return currentInput;
}






// Auxiliar output functions that show the state debug
void outputA()
{
	Serial.println("TASK CLAVE 1 2");
}

void outputB()
{
	Serial.println("TASK BLOQUEADO 0");
}

void outputC()
{
	Serial.println("TASK MONITOR TH 3 4");
}

void outputD()
{
	Serial.println("TASK ALARM 2");
}

void outputE()
{
	Serial.println("TASK LIGHT 2 3");
}