#ifndef HARDWARE_H
#define HARDWARE_H

#include "mbed.h"
#include "PpmRegen.h"
#include "PID.h"
#include "hcsr04.h"
#include "ConfigFile.h"


void loadConfigFile(void);
void writeSettingsToConfig(void);
void writeErrorLog(char *message);
void ConvertToCharArray(float number);
void ConvertToCharArray(int number);

Serial          pc(USBTX, USBRX); // tx, rx
PID*            _groundDistance;
//RtosTimer       *_groudRegulationUpdateTimer;
HCSR04*         _sonic;
uint8_t        _frontDistance;
uint8_t        _leftDistance;
uint8_t        _rightDistance;
uint8_t        _backDistance;
PpmRegen*       _ppmRegen;

ConfigFile _configFile;
LocalFileSystem local("local");

Semaphore _semaphore(1);
Thread  _serverThread;
Thread  _frontSensorThread;
Thread  _leftSensorThread;
Thread  _rightSensorThread;
Thread  _backSensorThread;
Thread  *_distanceThread;


InterruptIn*    _interruptPin =    new InterruptIn(p28);
PwmOut*         _roll =             new PwmOut(p21);
PwmOut*         _pitch =            new PwmOut(p22);
PwmOut*         _throttle =         new PwmOut(p23);
PwmOut*         _yaw =              new PwmOut(p24);
PwmOut*         _aux1 =             new PwmOut(p25);
PwmOut*         _aux2 =             new PwmOut(p26);


char*  _str = new char[1024];
char* _serverMessage = new char[1024];
bool _configChanges = false;
bool _onlyDistanChanged = false;
// zero is default value
float _P = 0;
float _I = 0;
float _D = 0;
int _groundSetPoint = 0;
int _bias = 0;
int _groundPidMinOutput = 0;
int _groundPidMaxOutput = 0;
bool _groundRegulation = false;
// Temporary values for Server to filter noise
float _newP;
float _newI;
float _newD;
int _newGroundSetPoint;
bool _tempGroundRegulation = false;
bool _goAhead = false;
bool _backWall = true;
bool _frontWall = false;







void loadConfigFile(void){
    //reading configFile
    _configFile.read("/local/config.cfg");
    char value[BUFSIZ];
    if (_configFile.getValue("bias", &value[0], sizeof(value)))
        _bias = atof(value);
    if (_configFile.getValue("groundPidMinOutput", &value[0], sizeof(value)))
        _groundPidMinOutput = atof(value);
    if (_configFile.getValue("groundPidMaxOutput", &value[0], sizeof(value)))
        _groundPidMaxOutput = atof(value);
    if (_configFile.getValue("P", &value[0], sizeof(value)))
        _P = atof(value);
    if (_configFile.getValue("I", &value[0], sizeof(value)))
        _I = atof(value);
    if (_configFile.getValue("D", &value[0], sizeof(value)))
        _D = atof(value);
    if (_configFile.getValue("groundSetPoint", &value[0], sizeof(value)))
        _groundSetPoint = atof(value);
        
}

void writeSettingsToConfig(void){
    ConvertToCharArray(_bias);
    if(!_configFile.setValue("bias", _str))
        strcat(_serverMessage, "bias value could not be saved to configFile\n\r");
    ConvertToCharArray(_groundPidMinOutput);
    if(!_configFile.setValue("groundPidMinOutput", _str))
        strcat(_serverMessage, "groundPidMinOutput value could not be saved to configFile\n\r");
    ConvertToCharArray(_groundPidMaxOutput);
    if(!_configFile.setValue("groundPidMaxOutput", _str))
        strcat(_serverMessage, "groundPidMaxOutput value could not be saved to configFile\n\r");
    ConvertToCharArray(_P);
    if(!_configFile.setValue("P", _str))
        strcat(_serverMessage, "P value could not be saved to configFile\n\r");
    ConvertToCharArray(_I);
    if(!_configFile.setValue("I", _str))
        strcat(_serverMessage, "I value could not be saved to configFile\n\r");
    ConvertToCharArray(_D);
    if(!_configFile.setValue("D", _str))
        strcat(_serverMessage, "D value could not be saved to configFile\n\r");
    ConvertToCharArray(_groundSetPoint);
    if(!_configFile.setValue("groundSetPoint", _str)){
        strcat(_serverMessage, "groundSetPoint value could not be saved to configFile\n\r");
        pc.printf("groundSetPoint value could not be saved to configFile\n\r");
    }
    _configFile.write("/local/config.cfg");        
    
}


void writeErrorLog(char *message){
    FILE *fp = fopen("/local/errorlog.txt", "w");  
    fprintf(fp, message);
    fclose(fp);
    
}


//Converts float to char array
void ConvertToCharArray(float number)
{
    sprintf(_str, "%3.2f", number );  
}

//Converts integer to char array
void ConvertToCharArray(int number)
{
    sprintf(_str, "%d", number );  
}


#endif