#include "mbed.h"
#include "rtos.h"
#include "hardware.h"


RawSerial esp(p13, p14); // tx, rx
DigitalOut  reset(p11);
DigitalOut  chd_dp(p10);

// Standard Mbed LED definitions
DigitalOut  led1(LED1);      // (PTB18)
DigitalOut  led3(LED3);     // (PTD1)

Timer t1;
Timer t2;

char cmdbuff[32];
char replybuff[1024];
char webcount[8];
char webdata[2048]; // This may need to be bigger depending on WEB browser used
char webbuff[8192];     // Currently using 1986 characters, Increase this if more web page data added
char type[16];
char channel[2];
char ip_address[20];
int bufflen, DataRX, count, getcount, replycount, servreq, timeout;
int bufl, ipdLen, linkID, weberror, webcounter;

void getreply(); void SendCMD();
void startserver();
void ReadWebData();
void sendpage();
void sendcheck();
void get_ip_address(char web_data[1024]);
void SendWEB();

void serverRun();
void serverMain();
void serverLoop();
void callback();

int port        = 80;  // set server port
int SERVtimeout = 8;    // set server timeout in seconds in case link breaks.



//serverThread.start(serverRun);

void callback(){
    ////pc.printf("callback!!!!!!!!!!!!!!!!_________________________\n\r");
    led3 = 1;
    while (esp.readable()) {
        webbuff[count] = esp.getc();
        count++;
    }
    if(strlen(webbuff)>bufflen) {
        //printf("bufflen %d \n\r");
        DataRX=1;
        led3=0;
    }
}


void serverRun(){
    chd_dp = 1;
    reset=0;
    
    //pc.printf("\f\n\r------------ ESP8266 Hardware Reset --------------\n\r");
    wait(0.5);
    reset=1;
    timeout=6000;
    getcount=500;
    getreply();
    esp.baud(115200);   // ESP8266 baudrate. Maximum on KLxx' is 115200, 230400 works on K20 and K22F
    startserver();
    webcounter = 0;
    Timer timer;
    bool pageWasSent = false;
    while(1){
        if(DataRX==1) {
            timer.start();
            ReadWebData();
            // Save data to configFile
            if ((servreq == 1 && weberror == 0) && pageWasSent == false) {
                    // send HTTP Response
                    sendpage();
                    pageWasSent = true;
                    timer.reset();
                    
            }
            else{
                pc.printf("Page was not sent \n\r");    
            }
            esp.attach(&callback);
            ////pc.printf(" IPD Data:\r\n\n Link ID = %d,\r\n IPD Header Length = %d \r\n IPD Type = %s\r\n", linkID, ipdLen, type);
            ////pc.printf("HTTP Packet: \n\n%s", webdata);
            ////pc.printf("Web Characters sent : %d\n\n", bufl);
            ////pc.printf("-------------------------------------\n\n");
            servreq=0;
            memset(_serverMessage, '\0', sizeof(_serverMessage));
        }
        if(timer.read_ms() > 3000){
            pageWasSent = false;
            ////pc.printf("starting reading new configuration in server task\n\r");
            // when stop sending answers
            timer.stop();
            timer.reset();
            if(_newP == _P && _newI == _I && _newD == _D){
                _onlyDistanChanged = true;      
            }
            _groundSetPoint = _newGroundSetPoint;
            _P = _newP;
            _I = _newI;
            _D = _newD;
            // Save data to configFile
            writeSettingsToConfig();
            // notification that config of pid was changed
            _configChanges = true;
            // start or stop ground regulation
            //_groundRegulation = _tempGroundRegulation;
            if(_groundRegulation != _tempGroundRegulation){
                _groundRegulation = _tempGroundRegulation;
                _groundDistance->resetError();
            }
            
        }
        
        Thread::wait(100);
    }
}


// Static WEB page
void sendpage(){
    strcpy(webbuff, "<!DOCTYPE html>");
    strcat(webbuff, "<html><head><title>ESP8266 Mbed LPC1768</title></head>");
    strcat(webbuff, "<body>");
    //strcat(webbuff, "<div style=\"text-align:center; background-color:#F4F4F4; color:#00AEDB;\"><h1>ESP8266 Mbed IoT Web PID Controller</h1>");
    //strcat(webbuff, "Hit Count - ");
    //strcat(webbuff, webcount);
    //strcat(webbuff, "</div><br /><hr>");
    //server message (mostly errors)
    //strcat(webbuff, "<div stye=\"text-align:center; background-color:#F4F4F4; color:#00AEDB;\"><h3>Server messages</h3>");
    //strcat(webbuff, _serverMessage);
    //strcat(webbuff, "</div><br><hr>");
    strcat(webbuff, "<form method=\"POST\">");
    //if(led1==0) {
    //    strcat(webbuff, "<p><input type=\"radio\" name=\"led1\" value=\"0\" checked>  LED 1 off");
    //    strcat(webbuff, "<br><input type=\"radio\" name=\"led1\" value=\"1\" >  LED 1 on<br>");
    //} else {
    //    strcat(webbuff, "<p><input type=\"radio\" name=\"led1\" value=\"0\" >  LED 1 off");
    //    strcat(webbuff, "<br><input type=\"radio\" name=\"led1\" value=\"1\" checked>  LED 1 on<br>");
    //}
    // pid ground min output
    strcat(webbuff, "<table><tr>");
    strcat(webbuff, "<td>PID min: </td> <td><input type=\"text\" name=\"groundPidMinOutput\" size=\"4\" value=\"");
    ConvertToCharArray(_groundPidMinOutput);
    strcat(webbuff, _str);
    strcat(webbuff, "\">");
    strcat(webbuff, "</td></tr>");
    strcat(webbuff, "<tr>");
    //pid ground max output
    strcat(webbuff, "<td>PID max: </td><td> <input type=\"text\" name=\"groundPidMaxOutput\" size=\"4\" value=\"");
    ConvertToCharArray(_groundPidMaxOutput);
    strcat(webbuff, _str);
    strcat(webbuff, "\">");
    strcat(webbuff, "</td></tr>");
    //ground regulation bias
    strcat(webbuff, "<tr>");
    strcat(webbuff, "<td>Bias: </td><td><input type=\"text\" name=\"bias\" size=\"4\" value=\"");
    ConvertToCharArray(_bias);
    strcat(webbuff, _str);
    strcat(webbuff, "\">");
    strcat(webbuff, "</td></tr>");
    strcat(webbuff, "<tr>");
    strcat(webbuff, "<td>P:</td><td> <input type=\"text\" name=\"proportional\" size=\"4\" value=\"");
    ConvertToCharArray(_newP);
    strcat(webbuff, _str);
    strcat(webbuff, "\"></td></tr>");
    strcat(webbuff, "<tr>");
    strcat(webbuff, "<td>I: </td><td><input type=\"text\" name=\"integral\" size=\"4\" value=\"");
    ConvertToCharArray(_newI);
    strcat(webbuff, _str);
    strcat(webbuff, "\"></td></tr>");
    strcat(webbuff, "<tr>");
    strcat(webbuff, "<td>D: </td><td><input type=\"text\" name=\"derivative\" size=\"4\" value=\"");
    ConvertToCharArray(_newD);
    strcat(webbuff, _str);
    strcat(webbuff, "\"></td></tr>");
    strcat(webbuff, "</table><br>");
    //Ground set Point
    strcat(webbuff, "Regulovana vyska: <input type=\"text\" name=\"groundSetPoint\" size=\"4\" value=\"");
    ConvertToCharArray(_newGroundSetPoint);
    strcat(webbuff, _str);
    strcat(webbuff, "\"><br>");
    // ground regulation
    if(_tempGroundRegulation) {
        strcat(webbuff, "<p><input type=\"radio\" name=\"groundRegulation\" value=\"1\" checked>  Regulace vysky - zapnout");
        strcat(webbuff, "<br><input type=\"radio\" name=\"groundRegulation\" value=\"0\" >  Regulace vysky - vypnout</p>");
    } else {
        strcat(webbuff, "<p><input type=\"radio\" name=\"groundRegulation\" value=\"1\" >  Regulace vysky - zapnout");
        strcat(webbuff, "<br><input type=\"radio\" name=\"groundRegulation\" value=\"0\" checked>  Regulace vysky - vypnout<p>");
    }
    if(_goAhead == false) {
        strcat(webbuff, "<p><input type=\"radio\" name=\"goAhead\" value=\"1\" >  Autonomni pohyb - zapnout");
        strcat(webbuff, "<br><input type=\"radio\" name=\"goAhead\" value=\"0\" checked>  Autonomni pohyb - vypnout</p>");
    } else {
        strcat(webbuff, "<p><input type=\"radio\" name=\"goAhead\" value=\"1\" checked>  Autonomni pohyb - zapnout");
        strcat(webbuff, "<br><input type=\"radio\" name=\"goAhead\" value=\"0\"> Autonomni pohyb - vypnout</p>");
    }
    strcat(webbuff, "<p><input type=\"radio\" name=\"nothing\" value=\"1\" style=\"display:none\" checked></p>");
    strcat(webbuff, "<p><input type=\"submit\"></p>");
    strcat(webbuff, "</form>");
    strcat(webbuff, "</body></html>");
// end of WEB page data
    bufl = strlen(webbuff); // get total page buffer length
    sprintf(cmdbuff,"AT+CIPSEND=%d,%d\r\n", linkID, bufl); // send IPD link channel and buffer character length.
    pc.printf("%s \n\r", cmdbuff);
    timeout=200;
    getcount=7;
    SendCMD();
    Thread::wait(1000);
    getreply();
    pc.printf("%s \n\r", replybuff);
    SendWEB();  // send web page
    memset(webbuff, '\0', sizeof(webbuff));
    sendcheck();
   
}

void sendcheck(){
    weberror=1;
    timeout= 1000;
    getcount = 50;
    t2.reset();
    t2.start();
    while(weberror==1 && t2.read() <5) {
        getreply();
        if (strstr(replybuff, "SEND OK") != NULL) {
            weberror=0;   // wait for valid SEND OK
        }
    }
    if(weberror==1) { // restart connection
        pc.printf("sendcheck - restarting connection \n\r");
        strcpy(cmdbuff, "AT+CIPMUX=1\r\n");
        timeout=500;
        getcount=10;
        SendCMD();
        getreply();
        //pc.printf(replybuff);
        sprintf(cmdbuff,"AT+CIPSERVER=0,%d\r\n", port);
        timeout=500;
        getcount=10;
        SendCMD();
        getreply();
        //pc.printf(replybuff);
        sprintf(cmdbuff,"AT+CIPSERVER=1,%d\r\n", port);
        timeout=500;
        getcount=10;
        SendCMD();
        getreply();
        //pc.printf(replybuff);
    } else {
        //pc.printf("sendcheck ok \n\r");
        sprintf(cmdbuff, "AT+CIPCLOSE=%s\r\n",channel); // close current connection
        SendCMD();
        getreply();
        //pc.printf(replybuff);
    }
    t2.reset();
}

// Large WEB buffer data send
void SendWEB()
{
    int i=0;
    if(esp.writeable()) {
        while(webbuff[i]!='\0') {
            esp.putc(webbuff[i]);
            i++;
        }
    }
}

// Reads and processes GET and POST web data
void ReadWebData(){
    wait_ms(200);
    esp.attach(NULL);
    count=0;
    DataRX=0;
    weberror=0;
    memset(webdata, '\0', sizeof(webdata));
    int x = strcspn (webbuff,"+");
    if(x) {
        strcpy(webdata, webbuff + x);
        //////pc.printf("webdata received: %s", webdata);
        weberror=0;
        int numMatched = sscanf(webdata,"+IPD,%d,%d:%s", &linkID, &ipdLen, type);
        if (strstr(webdata, "led1=1") != NULL ) {
            led1=1;
        }
        if (strstr(webdata, "led1=0") != NULL ) {
            led1=0;
        }
        //ground regulation
        if (strstr(webdata, "groundRegulation=1") != NULL )
            _tempGroundRegulation = true;
        if (strstr(webdata, "groundRegulation=0") != NULL )
            _tempGroundRegulation = false;
        // pid min Output
        if (strstr(webdata, "groundPidMinOutput") != NULL ){
            //////pc.printf("\n\r looking for data \n\r");
            char* p_webdata = strstr(webdata, "groundPidMinOutput");
            p_webdata = p_webdata + strlen("groundPidMinOutput") + 1;
            int i = 0;
            while(*p_webdata != '&'){
                ////pc.printf("%c", *p_webdata);
                _str[i] = *p_webdata;
                p_webdata += 1;   
                i++; 
            }
            ////pc.printf("\n\r");
            _str[i] = '\0';
            //////pc.printf("groundPidMinOutput: %s", _str);
            _groundPidMinOutput = atof(_str);
            //////pc.printf("\n\r end of looking for data \n\r");
        }
        //pid max output
        if (strstr(webdata, "groundPidMaxOutput") != NULL ){
            //////pc.printf("\n\r looking for data \n\r");
            char* p_webdata = strstr(webdata, "groundPidMaxOutput");
            p_webdata = p_webdata + strlen("groundPidMaxOutput") + 1;
            int i = 0;
            while(*p_webdata != '&'){
                ////pc.printf("%c", *p_webdata);
                _str[i] = *p_webdata;
                p_webdata += 1;   
                i++; 
            }
            //////pc.printf("\n\r");
            _str[i] = '\0';
            //////pc.printf("groundPidMaxOutput: %s", _str);
            _groundPidMaxOutput = atof(_str);
            //////pc.printf("\n\r end of looking for data \n\r");
        }
        //bias
        if (strstr(webdata, "bias") != NULL ){
            //////pc.printf("\n\r looking for data \n\r");
            char* p_webdata = strstr(webdata, "bias");
            p_webdata = p_webdata + strlen("bias") + 1;
            int i = 0;
            while(*p_webdata != '&'){
                ////pc.printf("%c", *p_webdata);
                _str[i] = *p_webdata;
                p_webdata += 1;   
                i++; 
            }
            //////pc.printf("\n\r");
            _str[i] = '\0';
            //////pc.printf("bias: %s", _str);
            _bias = atof(_str);
            //////pc.printf("\n\r end of looking for data \n\r");
        }
        // PID values
        if (strstr(webdata, "proportional") != NULL ){
            //////pc.printf("\n\r looking for data \n\r");
            char* p_webdata = strstr(webdata, "proportional");
            p_webdata = p_webdata + strlen("proportional") + 1;
            int i = 0;
            while(*p_webdata != '&'){
                ////pc.printf("%c", *p_webdata);
                _str[i] = *p_webdata;
                p_webdata += 1;   
                i++; 
            }
            //////pc.printf("\n\r");
            _str[i] = '\0';
            //////pc.printf("proportional: %s", _str);
            _newP = atof(_str);
            //////pc.printf("\n\r end of looking for data \n\r");
        }
        if (strstr(webdata, "integral") != NULL){
            //////pc.printf("\n\r looking for data \n\r");
            char* p_webdata = strstr(webdata, "integral");
            p_webdata = p_webdata + strlen("integral") + 1;
            int i = 0;
            while(*p_webdata != '&'){
                ////pc.printf("%c", *p_webdata);
                _str[i] = *p_webdata;
                p_webdata += 1;   
                i++; 
            }
            //////pc.printf("\n\r");
            _str[i] = '\0';
            //////pc.printf("integral: %s", _str);
            _newI = atof(_str);
            //////pc.printf("\n\r end of looking for data \n\r");    
            
        }
        if (strstr(webdata, "derivative") != NULL){
            //////pc.printf("\n\r looking for data \n\r");
            char* p_webdata = strstr(webdata, "derivative");
            p_webdata = p_webdata + strlen("derivative") + 1;
            int i = 0;
            while(*p_webdata != '&'){
                ////pc.printf("%c", *p_webdata);
                _str[i] = *p_webdata;
                p_webdata += 1;   
                i++; 
            }
            //////pc.printf("\n\r");
            _str[i] = '\0';
            //////pc.printf("derivative: %s", _str);
            _newD = atof(_str);
            //////pc.printf("\n\r end of looking for data \n\r");       
        }
        if (strstr(webdata, "groundSetPoint") != NULL){
            //////pc.printf("\n\r looking for data \n\r");
            char* p_webdata = strstr(webdata, "groundSetPoint");
            p_webdata = p_webdata + strlen("groundSetPoint") + 1;
            int i = 0;
            while(*p_webdata != '&'){
                ////pc.printf("%c", *p_webdata);
                _str[i] = *p_webdata;
                p_webdata += 1;   
                i++; 
            }
            //////pc.printf("\n\r");
            _str[i] = '\0';
            //////pc.printf("groundSetPoint: %s", _str);
            _newGroundSetPoint = atof(_str);
            //////pc.printf("\n\r end of looking for data \n\r"); 
        }
        //Go ahead
        if (strstr(webdata, "goAhead=1") != NULL ){
            pc.printf("go agead ON \n\r");   
            _goAhead = true;
        }
        if (strstr(webdata, "goAhead=0") != NULL ){
            pc.printf("go agead OFF \n\r");
            _goAhead = false;
        }
        pc.printf("test \n\r");
        sprintf(channel, "%d",linkID);
        if (strstr(webdata, "GET") != NULL) {
            pc.printf("GET \n\r");
            servreq=1;
        }
        if (strstr(webdata, "POST") != NULL) {
            pc.printf("POST \n\r");
            servreq=1;
        }
        webcounter++;
        sprintf(webcount, "%d",webcounter);
    }else{
        pc.printf("no + \n\r");
        memset(webbuff, '\0', sizeof(webbuff));
        esp.attach(&callback);
        weberror=1;        
    }    
    
}

void startserver(){
    //pc.printf("++++++++++ Resetting ESP ++++++++++\r\n");
    strcpy(cmdbuff,"AT+RST\r\n");
    timeout=8000;
    getcount=1000;
    SendCMD();
    getreply();  
    //pc.printf(replybuff);
    //pc.printf("%d",count);
    if (strstr(replybuff, "OK") != NULL) {
        //////pc.printf("\n++++++++++ Starting Server ++++++++++\r\n");
        strcpy(cmdbuff, "AT+CIPMUX=1\r\n");  // set multiple connections.
        timeout=500;
        getcount=20;
        SendCMD();
        getreply();
        //pc.printf(replybuff);
        sprintf(cmdbuff,"AT+CIPSERVER=1,%d\r\n", port);
        timeout=500;
        getcount=20;
        SendCMD();
        getreply();
        //pc.printf(replybuff);
        wait(1);
        sprintf(cmdbuff,"AT+CIPSTO=%d\r\n",SERVtimeout);
        timeout=500;
        getcount=50;
        SendCMD();
        getreply();
        //pc.printf(replybuff);
        wait(5);
        //pc.printf("\n Getting Server IP \r\n");
        strcpy(cmdbuff, "AT+CIFSR\r\n");
        timeout=2500;
        getcount=200;  
        while(weberror==0) {
            SendCMD();
            getreply();
            if (strstr(replybuff, "0.0.0.0") == NULL) {
                weberror=1;   // wait for valid IP
            }
        }
        //pc.printf("\n Enter WEB address (IP) found below in your browser \r\n\n");
        //pc.printf("\n The MAC address is also shown below,if it is needed \r\n\n");
        replybuff[strlen(replybuff)-1] = '\0';
        //char* IP = replybuff + 5;
        sprintf(webdata,"%s", replybuff);
        //SAVE IP ADDRESS
        get_ip_address(webdata);
        //pc.printf("ip_address: %s\n\r",ip_address);
        //pc.printf(webdata);
        //pc.printf("\n\n++++++++++ Ready ++++++++++\r\n\n");
        esp.attach(&callback);
    } else {
        //pc.printf("\n++++++++++ ESP8266 error, check power/connections ++++++++++\r\n");
        while(1) {}
    }
    t2.reset();
    t2.start();
}

void SendCMD()
{
    esp.printf("%s", cmdbuff);
}

void getreply()
{
    memset(replybuff, '\0', sizeof(replybuff));
    t1.reset();
    t1.start();
    replycount=0;
    while(t1.read_ms()< timeout && replycount < getcount) {
        if(esp.readable()) {
            replybuff[replycount] = esp.getc();
            replycount++;
        }
    }
    t1.stop();
}

void get_ip_address(char web_data[1024]){
    char pre_text[] = "+CIFSR:STAIP,\"";
    char* p_text = strstr(webdata, pre_text);
    p_text = p_text+strlen(pre_text);
    char* p_text2 = strstr(p_text, "\"");
    for(int i = 0;;i++){
        if(*(p_text+i) == *p_text2){
            ip_address[i] = '\0';
            break;
        }
        ip_address[i] = *(p_text+i);
            
    }
}
