#include "mbed.h"
#include "rtos.h"
#include "hardware.h"
#include "Server.h"
#include "PpmRegen.h"
#include "distanceRegulation.h"
#include "hcsr04.h"
#include "front_back_sensors.h"
#include "PID.h"
#include "ConfigFile.h"

//TEMP


//VARIABLES
uint16_t ppmInChannelsValue[CHANNELS];


//FUNCTIONS
void print_ppmIn(void);


int main(){
    pc.baud(115200);
    
    osThreadId mainID;
    mainID = Thread::gettid();
    pc.printf("main gettid 0x%08X \n\r", mainID);
    
    // INITIALIZE CLASSES
    _ppmRegen = new PpmRegen(_interruptPin);
    float rate = (1.0 / FLIGHT_CONTROLLER_FREQUENCY);
    _groundDistance = new PID(0, 0, 0, rate);
    
    //setup PID
    loadConfigFile();
    _newP = _P;
    _newI = _I;
    _newD = _D;
    _newGroundSetPoint = _groundSetPoint;
    // to show values in web browser after first load
    _groundDistance->setInputLimits(0, 300);
    _groundDistance->setMode(AUTO_MODE);
    _groundDistance->setTunings(_P, _I, _D);
    _groundDistance->setOutputLimits(_groundPidMinOutput, _groundPidMaxOutput);
    _groundDistance->setBias(_bias);

    // STARTING THREADS
    pc.printf("starting distance thread \n\r");
    _distanceThread = new Thread(distanceRegulationThread);
    osThreadId distanceID;
    pc.printf("distance gettid 0x%08X \n\r", distanceID);
    _distanceThread->set_priority(osPriorityRealtime);
    
    //_sonic = new HCSR04(p7, p8, p5, p6);
    _sonic = new HCSR04(p5, p6);
    
    //_frontSensorThread.start(front_sensor);
    //_frontSensorThread.set_priority(osPriorityHigh);
    
    //_leftSensorThread.start(left_sensor);
    //_leftSensorThread.set_priority(osPriorityHigh);
    
    pc.printf("Starting sonic threads \n\r");
    _frontSensorThread.start(callback(semaphore_thread, (void *)"frontSensor"));
    _backSensorThread.start(callback(semaphore_thread, (void *)"backSensor"));
    _frontSensorThread.set_priority(osPriorityHigh);
    _backSensorThread.set_priority(osPriorityHigh);
    
    
    
    
    pc.printf("starting server thread \n\r");
    osThreadId serverID;
    pc.printf("server gettid 0x%08X \n\r", serverID);
    _serverThread.start(serverRun);
    _serverThread.set_priority(osPriorityHigh);
    
    //distanceRegulation.start(2);
    //_distanceThread.start(distanceRegulationTask);
    
    //wait(1);
    
    
    while(1){
        Thread::wait(osWaitForever);
    }
        
}


void print_ppmIn(){
    _ppmRegen->getAllChannels(ppmInChannelsValue);
    for(uint8_t channel= 0; channel < CHANNELS; channel++){
        pc.printf("PPM In Channel %d: %d \n\r", channel, ppmInChannelsValue[channel]);
    }
    pc.printf("\n\r");
}
