// Author  : Rob Musquetier 
// Date    : 9-1-2016
// Version : 1.0 First production version

// Include needed libraries
#include <OneWire.h>                      // OneWire Library for reading the DS18B20 temperature sensor
#include <DallasTemperature.h>            // Library for interpreting the DS18B20 readings
#include <DHT.h>                          // Library for reading the DHT22 temperature/humidity sensor
#include <Stepper.h>                      // Library for using the stepper motors

// Switch to add additional debugging (rem the line to disable the extra debugging)
#define DBG_SWITCH                      // Remove remark slashes to enable extensive debugging

// Set fix variable for serial communication
#define MAX_COMMAND_LEN             (7)   // Maximum length of the received commands
#define MAX_PARAMETER_LEN           (10)  // Maximum length of the received parameters
#define COMMAND_TABLE_SIZE          (15)  // Number of defined commands

// Function to transform strings to uppercase
#define TO_UPPER(x) (((x >= 'a') && (x <= 'z')) ? ((x) - ('a' - 'A')) : (x)) 

// Aperture control range
#define ARANGE                      (1300) // Range to go from aperture closed to open

// ASC variables initiation
byte  HeaterPin            = 12;          // Define heater control pin
byte  LightPin             = 0;           // Define anolog light detection pin

byte  OutsideTempPin       = 2;           // Define outside temperature sensor pin (D2)
byte  InsideTempPin        = 3;           // Define inside temperature sensor pin (D3)

byte  AStepperPin1         = 8;           // Define aperture stepper motor pin 1
byte  AStepperPin2         = 10;          // Define aperture stepper motor pin 2
byte  AStepperPin3         = 9;           // Define aperture stepper motor pin 3
byte  AStepperPin4         = 11;          // Define aperture stepper motor pin 4

byte  FStepperPin1         = 4;           // Define focus stepper motor pin 1
byte  FStepperPin2         = 6;           // Define focus stepper motor pin 2
byte  FStepperPin3         = 5;           // Define focus stepper motor pin 3
byte  FStepperPin4         = 7;           // Define focus stepper motor pin 4

// Initiate the DHT22 temperature/humidity sensor library 
#define DHTTYPE DHT22                     // Instruct to use the DHT22 outside temperature sensor
DHT dht(OutsideTempPin, DHTTYPE);         // Initiate object to control the DHT22

// Inititiate one wire read protocol for inside temperature sensor
OneWire oneWire(InsideTempPin);           // Initiate object to read the attached one wire objects (BS18B20)
DallasTemperature sensors(&oneWire);

// Initiate the stepper motor objects for the aperature and focus control stepper motors
Stepper ApertureMotor(32, AStepperPin1, AStepperPin2, AStepperPin3, AStepperPin4); // Initiate aperture stepper motor
Stepper FocusMotor(32, FStepperPin1, FStepperPin2, FStepperPin3, FStepperPin4);    // Initiate focus stepper motor

// Set temperature measure timer so it will determine if the heater should be adjusted
long t = 60000;

// Aperature control variables
long    AperturePos        = 0;           // Variable to store the aperture control position
#define ApertureSpeed      (100)          // Variable to set the speed for the aperture stepper motor

// Focus control variables
long    FocusPos           = 0;           // Variable to store the focus control position
#define FocusSpeed         (100)          // Variable to set the speed for the focus stepper motor

// Outside temperature and humidity variables
float   OHum               = 0.0;         // Variable containing the last read outside humidity value (%)
float   OTemp              = 0.0;         // Variable containing the last read outside temperature value (Celsius)
float   DewPoint           = 0.0;         // Variable containing the last calculated dew point value (Celsius)

// Inside temperature and humidity variables
float   ITemp              = 0.0;         // Variable containing the last read inside temperature value (Celsius)

// Heater control variables
int     PWMCounter         = 0;           // PWM emulator counter
int     HeaterValue        = 20;          // Variable containing the needed heater value (0 - 255)
boolean HeaterFlag         = false;       // Indicates if heater is on (within PWM signal)
#define HeaterStepSize     (5);           // Determines the step size of the heater increase or decrease

// Light sensor variables
boolean LightFlag          = true;        // Flag to indicate if it is light outside (default true meaning it is light outside)
int     OLight             = 1023;        // Variable to hold the light value (higher is lighter (range app. 850 - 1023))
#define DarkLevel          (650)           // Analog value for dark
#define LightLevel         (350)            // Analog value for light

// Serial communication variables
char    incomingByte       = 0;           // Serial in data byte
byte    serialValue        = 0;           // Reset serial data string flag
boolean usingSerial        = true;        // Set to false to have the buttons control everything

char gCommandBuffer[MAX_COMMAND_LEN + 1]; // String for received commands
char gParamBuffer[MAX_PARAMETER_LEN + 1]; // String for received parameter
long gParamValue;                         // Received parameter

// Update flags. used to update states after receiving a command
boolean UPDATE                 = true;    // Flag to indicate data should be read
boolean UPDATEApertureStepIn   = false;   // Flag to indicate aperture step in was made
boolean UPDATEApertureStepOut  = false;   // Flag to indicate aperture step out was made
boolean UPDATEGetAperturePos   = false;   // Flag to indicate aperture position needs to be sent
boolean UPDATEResetAperturePos = false;   // Flag to indicate aperture position needs to be resetted
boolean UPDATEFocusStepIn      = false;   // Flag to indicate focus step in was made
boolean UPDATEFocusStepOut     = false;   // Flag to indicate focus step out was made
boolean UPDATEGetFocusPos      = false;   // Flag to indicate focus position needs to be sent
boolean UPDATEResetFocusPos    = false;   // Flag to indicate focus position needs to be resetted
boolean UPDATEDewPoint         = false;   // Flag to indicate dew point calcualtion was made 
boolean UPDATEOutsideHum       = false;   // Flag to indicate outside humidity value was read
boolean UPDATEOutsideTemp      = false;   // Flag to indicate outside temperature was read
boolean UPDATEInsideTemp       = false;   // Flag to indicate inside temperature was read
boolean UPDATEGetHeaterValue   = false;   // Flag to indicate heater value needs to be send
boolean UPDATEHeater           = true;    // Flag to indicate heater value was updated
boolean UPDATEGetLightValue    = false;   // Flag to indicate light value needs to be send

// Define structure for serial read commands
boolean f                      = false;
typedef struct {                          
	char const    *name;
	void          (*function)(void);
}
command_t;

// Set up a command table. When the command is sent from the PC this table points it to the subroutine to run
command_t const gCommandTable[COMMAND_TABLE_SIZE] = {
  { "AIN",    ApertureStepIn, },          // Command to close aperture
  { "AOUT",   ApertureStepOut, },         // Command to open aperture
  { "APOS",   GetAperturePosition, },     // Command to retrieve aperture stepper motor position
  { "ARST",   ResetAperturePosition, },   // Command to reset aperture stepper motor position
  { "FIN",    FocusStepIn, },             // Command to focus in
  { "FOUT",   FocusStepOut, },            // Command to focus out
  { "FPOS",   GetFocusPosition, },        // Command to retrieve focus stepper motor position
  { "FRST",   ResetFocusPosition, },      // Command to reset focus stepper motor position
  { "DEWP",   GetDewPoint, },             // Command to retrieve duw point
  { "OHUM",   GetOutsideHumidity, },      // Command to retrieve outside humidity level
  { "OTMP",   GetOutsideTemperature, },   // Command to retrieve outside temperature
  { "ITMP",   GetInsideTemperature, },    // Command to retrieve inside temperature
  { "GHV",    GetHeaterValue, },          // Command to retrieve heater value
  { "GLV",    GetLightValue, },           // Command to retrieve light value
	{ NULL,    NULL   }
};
// Serial Comms setup end

// Start of setup routine
void setup() {
  
  pinMode(InsideTempPin,  INPUT);         // Set inside temperature pin as input
  pinMode(OutsideTempPin, INPUT);         // Set outside temperature pin as input

  pinMode(HeaterPin,      OUTPUT);        // Set heater pin as output
  digitalWrite(HeaterPin, LOW);

  pinMode(AStepperPin1,   OUTPUT);        // Set aperture stepper motor pin 1 as output
  pinMode(AStepperPin2,   OUTPUT);        // Set aperture stepper motor pin 2 as output
  pinMode(AStepperPin3,   OUTPUT);        // Set aperture stepper motor pin 3 as output
  pinMode(AStepperPin4,   OUTPUT);        // Set aperture stepper motor pin 4 as output
  
  pinMode(FStepperPin1,   OUTPUT);        // Set focus stepper motor pin 1 as output
  pinMode(FStepperPin2,   OUTPUT);        // Set focus stepper motor pin 2 as output
  pinMode(FStepperPin3,   OUTPUT);        // Set focus stepper motor pin 3 as output
  pinMode(FStepperPin4,   OUTPUT);        // Set focus stepper motor pin 4 as output

  // Wait until serial bus is available
  while (!Serial);

  // Initiate serial bus for 19200 baud usage
	Serial.begin (19200);

  // Set initial motor speed for aperture and focus stepper motor
  ApertureMotor.setSpeed(ApertureSpeed);
  FocusMotor.setSpeed(FocusSpeed);

  // Setup read protocol for communication with the outside temperature/humidity sensor
  dht.begin();

  // Setup one wire read protocol for one wire communication with the inside temperature sensor
  sensors.begin();
}
// End of setup routing

// Start of infinitive loop
void loop() {

  // Indicate no command has been received
	int bCommandReady = false;

  // Wait 100ms
  delay(1);

  // Increate heater skip counter
  t++;
  
  // Check if 60000x 1ms has been passed
  if (t > 6000) {
    // Reset counter
    t = 0;

    // Determine DewPoint
    f = true;
    GetDewPoint();

    // Get inside temperature
    f = true;
    GetInsideTemperature();
      #ifdef DBG_SWITCH
        Serial.print("Inside Temperature: ");
        Serial.print(ITemp);
        Serial.println("C");
      #endif

    // Check if inside temperature is at least 0.5 degrees above the calculated dew point and if heater is not on maximum capacity yet
    if (((ITemp + 0.5) < DewPoint) && (HeaterValue < 255)) {
      #ifdef DBG_SWITCH
        Serial.println("Heater value to low!");
      #endif

      // Increase heater PWM value
      HeaterValue = HeaterValue + HeaterStepSize;

      // Ensure value doesn't exceed 255
      if (HeaterValue > 255)
        HeaterValue = 255;

      // Indicate heater value was adjusted
      UPDATEHeater = true;
    }

    // Check if inside temperature is at max 1.5 degrees above the calculated dew point and if heater is not off yet
    if (((ITemp + 1.5) > DewPoint) && (HeaterValue > 0)) {
      #ifdef DBG_SWITCH
        Serial.println("Heater value to high!");
      #endif

      // Decrease heater PWM value
      HeaterValue = HeaterValue - HeaterStepSize;

      // Ensure value doesn't drop below zero
      if (HeaterValue < 0)
        HeaterValue = 0;

      // Indicate heater value was adjusted
      UPDATEHeater = true;
    }

    // Check if heater value was adjusted
    if (UPDATEHeater) {
      #ifdef DBG_SWITCH
        Serial.print("Change heater value to: ");
        Serial.print(HeaterValue / 2.55);
        Serial.println("%");
      #endif

      // Reset heater value adjust flag
      UPDATEHeater = false;
    }

    // Get value of the Light Sensor
    OLight = analogRead(A0);
    #ifdef DBG_SWITCH
      Serial.print("Measured Light Value [< ");
      Serial.print(LightLevel);
      Serial.print(" = Light, > ");
      Serial.print(DarkLevel);
      Serial.print(" = Dark]: ");
      Serial.print(OLight);
      Serial.print(", status Aperture control: ");
      if (LightFlag)
        Serial.println("Open");
      else
        Serial.println("Closed");
    #endif

    // Check if it is light outside
    if (OLight < LightLevel and !LightFlag) {
      #ifdef DBG_SWITCH
        Serial.println("Light Outside, closing aperture...");
      #endif

      gParamValue = ARANGE;
      ApertureStepIn();

      LightFlag = true;
    }

    // Check if it is dark outside
    if (OLight > DarkLevel and LightFlag) {
      #ifdef DBG_SWITCH
        Serial.println("Dark Outside, opening aperture...");
      #endif

      gParamValue = ARANGE;
      ApertureStepOut();

      LightFlag = false;
    }
  }
  // End of check if 60000x 1ms has been passed

  // Increase PWM cycle counter
  PWMCounter = PWMCounter + 1;

  // Check if PWM counter exceeds 255
  if (PWMCounter > 255)
    // Resete PWM cycle counter 0
    PWMCounter = 0;

  // Check if PWM counter has exceeded the set Heater Value and if Heaterpin is HIGH
  if (PWMCounter > HeaterValue && HeaterFlag) {

    // Switch heater pin to LOW
    digitalWrite(HeaterPin, LOW);

    // Indicate heater pin is resetted
    HeaterFlag = false;
  }

  // Check if PWM counter is lower than the set Heater Value and if Heaterpin is LOW
  if (PWMCounter <= HeaterValue && !HeaterFlag) {

    // Switch heater pin to HIGH
    digitalWrite(HeaterPin, HIGH);
    
    // Indicate heater pin is set
    HeaterFlag = true;
  }

  // Indicate some values might be updated
	UPDATE = true;

	// Check if There is information in the werial buffer, if so read it in and start the build command subroutine
	if (usingSerial && Serial.available() >= 1) {
		#ifdef DBG_SWITCH
			Serial.println("Something in the buffer found!!!");
		#endif

		// Read the incoming byte:
		incomingByte = Serial.read();

    // Wait 5 ms
		delay(5);
		
		// Check for start of command
		if (incomingByte == '#')
			// Build a new command
			bCommandReady = cliBuildCommand(incomingByte);
	}
  // Flag no commnad is started
	else
		incomingByte = 0;

	// Check if there is a command in the buffer then run the process command subroutine
	if (bCommandReady == true) {
    // Reset the command ready flag
		bCommandReady = false; 

		#ifdef DBG_SWITCH
			Serial.print("Received command: ");
			Serial.println(gCommandBuffer);

			Serial.print("Received value: ");
			Serial.println(gParamBuffer);
		#endif
    
    // Run the command
		cliProcessCommand(); 
	}

  // Flag command is processed
	if (UPDATE) {
		UPDATE = false;

    // Send the current state of the ASC to the PC over serial comms
		SerialDATAFun();  
	}
}
// End of infinitive loop

// Start of AllSkyCam fucntions

// Process command to execute aperture control step in
void ApertureStepIn (void) {

  // If no value was given default to 1
  if (!gParamValue)
    gParamValue = 1;

  // Turn stepper motor CCW
  ApertureMotor.step(gParamValue * -1);
  digitalWrite(AStepperPin1, LOW);
  digitalWrite(AStepperPin2, LOW);
  digitalWrite(AStepperPin3, LOW);
  digitalWrite(AStepperPin4, LOW);
  
  // Decrease aperture position variable
  AperturePos -= gParamValue;

  // Flag aperture control position has changed
  UPDATEApertureStepIn = true;
}

// Process command to execute aperture control step out
void ApertureStepOut (void) {

  // If no value was given default to 1
  if (!gParamValue)
    gParamValue = 1;

  // Turn stepper motor CW
  ApertureMotor.step(gParamValue);
  digitalWrite(AStepperPin1, LOW);
  digitalWrite(AStepperPin2, LOW);
  digitalWrite(AStepperPin3, LOW);
  digitalWrite(AStepperPin4, LOW);

  // Decrease aperture position variable
  AperturePos += gParamValue;

  // Flag aperture control position has changed
  UPDATEApertureStepOut = true;
}

// Get Aperture stepper motor position
void GetAperturePosition (void) {

  // Flag show aperture position 
  UPDATEGetAperturePos = true;
}

// Reset Aperture stepper motor position
void ResetAperturePosition (void) {

  // Reset aperture position variable
  AperturePos = 0;

  // Flag show aperture position 
  UPDATEResetAperturePos = true;
}

// Process command to execute focus control step in
void FocusStepIn (void) {

  // If no value was given default to 1
  if (!gParamValue)
    gParamValue = 1;

  // Turn stepper motor CCW
  FocusMotor.step(gParamValue * -1);
  digitalWrite(FStepperPin1, LOW);
  digitalWrite(FStepperPin2, LOW);
  digitalWrite(FStepperPin3, LOW);
  digitalWrite(FStepperPin4, LOW);

  // Decrease focus position variable
  FocusPos -= gParamValue;

  // Flag focus control position has changed
  UPDATEFocusStepIn = true;
}

// Process command to execute focus control step out
void FocusStepOut (void) {

  // If no value was given default to 1
  if (!gParamValue)
    gParamValue = 1;

  // Turn stepper motor one stap CW
  FocusMotor.step(gParamValue);
  digitalWrite(FStepperPin1, LOW);
  digitalWrite(FStepperPin2, LOW);
  digitalWrite(FStepperPin3, LOW);
  digitalWrite(FStepperPin4, LOW);

  // Increase focus position variable
  FocusPos += gParamValue;

  // Flag focus control position has changed
  UPDATEFocusStepOut = true;
}

// Get Focus stepper motor position
void GetFocusPosition (void) {

  // Flag show focus position 
  UPDATEGetFocusPos = true;
}

// Reset Focus stepper motor position
void ResetFocusPosition (void) {

  // Reset focus position variable
  FocusPos = 0;

  // Flag show focus position 
  UPDATEResetFocusPos = true;
}

// Calculate dew point (routine kindly borrowed from Gina)
void GetDewPoint (void) {

  // Define needed local variables
  double celsius, hum;

  // Read outside temperature
  celsius = dht.readTemperature();

  #ifdef DBG_SWITCH
    if (!isnan(celsius)) {
      Serial.print("Measured Outside Temperature: ");
      Serial.print(celsius);
      Serial.println("C");
    }
  #endif

  // Read outside humidity
  hum = dht.readHumidity();
  
  #ifdef DBG_SWITCH
    if (!isnan(hum)) {
      Serial.print("Measured Outside Humidity: ");
      Serial.print(hum);
      Serial.println("%");
    }
  #endif

  // Check if any reads failed and exit early (to try again).
  if (isnan(hum) || isnan(celsius)) {
    // Print error message on serial bus
    Serial.println("Failed to read temperature or humidity values from DHT sensor!");
    
    // Abort function
    return;
  }

  // Calculate tempature in Kelvin?
  DewPoint = celsius - ((100 - hum) / 5); // Simple calculation

  // Calculate dew point
  #ifdef DBG_SWITCH
    Serial.print("Calculated Dew Point: ");
    Serial.print(DewPoint);
    Serial.println("C");
  #endif
  
  // Inidcate new dew point has been determined
  if (!f)  
    UPDATEDewPoint = true;

  f = false;
}

// Function to get the outside temperature
void GetOutsideTemperature (void) {
  
  // Read temperature as Celsius (the default)
  OTemp = dht.readTemperature();

  // Check if any reads failed and exit early (to try again).
  if (isnan(OTemp)) {
    // Send error message to the serial bus
    Serial.println("Failed to read temperature value from DHT sensor!");
    
    // Abort the function
    return;
  }

  // Indicate outside temperature was read
  UPDATEOutsideTemp = true;
}

// Function to get the outside humidity
void GetOutsideHumidity (void) {
  
    // Reading temperature or humidity takes about 250 milliseconds!
  OHum = dht.readHumidity();
  
  // Check if any reads failed and exit early (to try again).
  if (isnan(OHum)) {
    // Send error message to the serial bus
    Serial.println("Failed to read humidity value from DHT sensor!");
    
    // Abort the function
    return;
  }

  // Indicate outside humidity level was read
  UPDATEOutsideHum = true;
}

// Function to get the inside temperature
void GetInsideTemperature (void) {

  // Send the command to get the temperature form the inside sensor
  sensors.requestTemperatures();

  // Readout the temperature of the first sensor
  ITemp = sensors.getTempCByIndex(0);

  // Indicate inside temperature was read
  if (!f)
    UPDATEInsideTemp = true;

  f = false;
}

// Function to get the inside temperature
void GetHeaterValue (void) {

  UPDATEGetHeaterValue = true;
}

// Function to get the light intensity value
void GetLightValue (void) {

  UPDATEGetLightValue = true;
}

// Update all information via the serial bus
void SerialDATAFun (void) { 
  // Check if aperature control changed
  if (UPDATEApertureStepIn) {
    // Send new position to the serial bus
    Serial.print("Aperture Motor Position: ");
    Serial.println(AperturePos); 

    // Indicate new aperture position was sent
    UPDATEApertureStepIn = false;
  }
  
  // Check if aperature control changed
  if (UPDATEApertureStepOut) {
    // Send new position to the serial bus
    Serial.print("Aperture Motor Position: ");
    Serial.println(AperturePos); 
    
    // Indicate new aperture position was sent
    UPDATEApertureStepOut = false;
  }

  // Check if aperature position needs to be send
  if (UPDATEGetAperturePos) {
    // Send new position to the serial bus
    Serial.print("Aperture Motor Position: ");
    Serial.println(AperturePos); 
    
    // Indicate new aperture position was sent
    UPDATEGetAperturePos = false;
  }

  // Check if aperature position was resetted
  if (UPDATEResetAperturePos) {
    // Send new position to the serial bus
    Serial.print("Aperture Motor Position: ");
    Serial.println(AperturePos); 
    
    // Indicate new aperture position was sent
    UPDATEResetAperturePos = false;
  }

  // Check if focus control changed
  if (UPDATEFocusStepIn) {
    // Send new position to the serial bus
    Serial.print("Focus Motor Position: ");
    Serial.println(FocusPos); 
    
    // Indicate new focus position was sent
    UPDATEFocusStepIn = false;
  }

  // Check if focus control changed
  if (UPDATEFocusStepOut) {
    // Send new position to the serial bus
    Serial.print("Focus Motor Position: ");
    Serial.println(FocusPos); 
    
    // Indicate new focus position was sent
    UPDATEFocusStepOut = false;
  }

  // Check if focus position needs to be send
  if (UPDATEGetFocusPos) {
    // Send new position to the serial bus
    Serial.print("Focus Motor Position: ");
    Serial.println(FocusPos); 
    
    // Indicate new aperture position was sent
    UPDATEGetFocusPos = false;
  }

  // Check if focus position was resetted
  if (UPDATEResetFocusPos) {
    // Send new position to the serial bus
    Serial.print("Focus Motor Position: ");
    Serial.println(FocusPos); 
    
    // Indicate new aperture position was sent
    UPDATEResetFocusPos = false;
  }

  // Check if dew point was determined
  if (UPDATEDewPoint) {
    // Send new dew point to the serial bus
    Serial.print("Dew Point: ");
    Serial.print(DewPoint); 
    Serial.println("C");
    
    // Indicate new dew point was sent
    UPDATEDewPoint = false;
  }

  // Check if outside temperature needs to be send
  if (UPDATEOutsideTemp) {
    // Send new outside temperature to the serial bus
    if (!isnan(OTemp)) {
      Serial.print("Outside Temperature: ");
      Serial.print(OTemp); 
      Serial.println("C");
    }    
    // Indicate new outside temperature was sent
    UPDATEOutsideTemp = false;
  }

  // Check if outside humidity value needs to be send
  if (UPDATEOutsideHum) {
    // Send new outside humidity to the serial bus
    if (!isnan(OHum)) {
      Serial.print("Outside Humidity: ");
      Serial.print(OHum); 
      Serial.println("%");
    }
    
    // Indicate new outside humidity was sent
    UPDATEOutsideHum = false;
  }

  // Check if inside temperature needs to be send
  if (UPDATEInsideTemp) {
    // Send new inside temperature to the serial bus
    Serial.print("Inside Temperature: ");
    Serial.print(ITemp); 
    Serial.println("C");
    
    // Indicate new inside temperature was sent
    UPDATEInsideTemp = false;
  }

  // Check if heater value needs to be send
  if (UPDATEGetHeaterValue) {
 
    // Send new inside temperature to the serial bus
    Serial.print("Heater Value: ");
    Serial.print(HeaterValue / 2.55); 
    Serial.println("%"); 
    
    // Indicate new inside temperature was sent
    UPDATEGetHeaterValue = false;
  }

  // Check if light value needs to be send
  if (UPDATEGetLightValue) {

    // Send new light value to the serial bus
    Serial.print("Light Value: ");
    Serial.println(OLight); 
    
    // Indicate new inside temperature was sent
    UPDATEGetHeaterValue = false;
  }
}
// End of AllSkyCam functions

// Start of serial communication control functions

// Process the received command function
void cliProcessCommand(void)
{
  // Initate and reset local command found flag
	int bCommandFound = false;
	
	// Initiate character index variable
	int idx;

	// Convert the parameter to an integer value. If the parameter is empty, gParamValue becomes 0
	gParamValue = strtol(gParamBuffer, NULL, 0);
/*
  #ifdef DBG_SWITCH
    Serial.print("Searching command: ");
    Serial.println(gCommandBuffer);
  #endif
*/  
	// Search for the command in the command table
	for (idx = 0; gCommandTable[idx].name != NULL; idx++) {
    // Check if command is found
		if (strcmp(gCommandTable[idx].name, gCommandBuffer) == 0) {
      // Indicate command was found
			bCommandFound = true;

      // Break out of command search loop
			break;
		}
	}

	// Check if command was found
	if (bCommandFound == true) {
/*    
		#ifdef DBG_SWITCH
			Serial.print("Executing command: ");
			Serial.print(gCommandTable[idx].name);
			Serial.print(": ");
			Serial.print(gParamValue);
			Serial.println(";");
		#endif
*/
    // Call the function for the identified received command
		(*gCommandTable[idx].function)();
	}
  // Command not found
	else {
		#ifdef DBG_SWITCH
			Serial.println("ERROR: Command not found: ");
			Serial.print(gCommandBuffer);
		#endif
	}
}
// End of process the received command function

// When data is in the Serial buffer this subroutine is run and the information put into a command buffer.
// The character # is used to define the start of a command string and the start of the parameter string
// The character : is used to define the end of a command string and the start of the parameter string
// The character ; is used to define the end of the parameter string
int cliBuildCommand(char nextChar) {
	// Initiate index variable for command buffer
	int idx = 0;  
	
	// Initiate index varaible for parameter buffer
	int idx2 = 0; 
/*
  #ifdef DBG_SWITCH
    Serial.print("Command length: ");
    Serial.println(MAX_COMMAND_LEN);
  #endif
*/
  // Clear command buffer
  gCommandBuffer[0] = '\0';
/*
	#ifdef DBG_SWITCH
		Serial.print("Read command: ");
	#endif
*/
  // Read first character of the command from the serial bus
	nextChar = Serial.read();

	// Start while loop
	do {
    // Wait 10us
		delayMicroseconds(10);
/*
		#ifdef DBG_SWITCH
      Serial.print("Character ");
      Serial.print(idx);
      Serial.print(" : ");
			Serial.println(nextChar);
      Serial.println(TO_UPPER(nextChar));
		#endif
*/

    // Convert character to upper case
		gCommandBuffer[idx] = TO_UPPER(nextChar);
  
    // Increate command buffer index counter
		idx++;

    // Read next character from th e serial bus
		nextChar = Serial.read();
	}
  // Loop until semi column is found or maximum length of command buffer is reached
	while ((nextChar != ':') && (idx < MAX_COMMAND_LEN));
/*
  #ifdef DBG_SWITCH
    Serial.print("Buffer length: ");
    Serial.println(MAX_PARAMETER_LEN);
  #endif
*/
  // Clear parameter buffer
  gParamBuffer[0] = '\0';
/*
	#ifdef DBG_SWITCH
		Serial.println("");
		Serial.print("Read parameter: ");
	#endif
*/
  // Read first character of parameter from the serial bus
	nextChar = Serial.read();

  // Start while loop
	do {
    // Wait 10us
		delayMicroseconds(10);
/*
		#ifdef DBG_SWITCH
			Serial.print(nextChar);
		#endif
*/
    // Check if received character is a numeric character
		if (nextChar >= 48 and nextChar <= 57) {
      // Add character to parameter buffer
			gParamBuffer[idx2] = nextChar;
		}
    else
      gParamBuffer[idx2] = '\0';
      
		// Increate parameter index buffer pointer variable
		idx2++;

    // Read next character of parameter from the serial bus
		nextChar = Serial.read();
	}
  // Loop until ; is found or index counter reaches the maximum length of the parameter command buffer
	while ((nextChar != ';') && (idx2 < MAX_PARAMETER_LEN));
/*	
	#ifdef DBG_SWITCH
		Serial.println("");
	#endif
*/
  // Add \0 character to the command buffer
	gCommandBuffer[idx] = '\0';
/*
	#ifdef DBG_SWITCH
		Serial.print("gCommandBuffer: ");
		Serial.println(gCommandBuffer);
	#endif
*/
  // Add \0 character to the parameter buffer
	gParamBuffer[idx2] = '\0';
/*
	#ifdef DBG_SWITCH
		Serial.print("gParamBuffer: ");
		Serial.println(gParamBuffer);
	#endif
*/
  // Exit function with a succes value
	return true;
}
// End of read command characters function

// End of serial control functions