#include <Arduino.h>
#include <ESP8266WiFi.h>
#include <ESP8266WebServer.h>
#include <Html.h>
#include <Graph.h>
#include <Sensor.h>
#include <WebSocketsServer.h>
#include <ArduinoJson.h>
#include "CpuLoad.h"
#include <Ticker.h>

#include "arduinoFFT.h"
arduinoFFT FFT = arduinoFFT();


const char *ssid = "LabWork_1";
ESP8266WebServer server(80);
Sensor accelerometer = Sensor();
Graph graphAcc = Graph(256, 50);
Ticker sensorTicker, stepTicker;


double vReal[256];
double vImag[256];

long int lastDataSend = 0;

int timeOfCal = 0;
bool accCalibrated = false;
int accCalRemains = 0;
int accMax1 = 0;
int accMax2 = 0;


double peakFreq = 0;
int steps = 0;

WebSocketsServer webSocket = WebSocketsServer(81);

void handleAccelerometer()
{
    int16_t bias = 0;
    const int arr_size = 8000;
    char *html_code = new char[arr_size];
    memset(html_code, '\0', sizeof(char) * arr_size);
    bias += getHtml(HTML_BEGIN, *html_code, arr_size, 0);
    server.send(200, "text/html", html_code);
    delete[] html_code;
}

void cookAccelerometer(String &in, int *in_array, int array_size, String arrayName) {
    String *output = &in;
    int *array = in_array;
    DynamicJsonBuffer jsonBuffer;
    JsonObject& root = jsonBuffer.createObject();
    JsonArray& data = root.createNestedArray(arrayName);
    for (int i = 0; i != array_size; i++) {
        data.add(*(array + i));
    }
    root.printTo(*output);
}

void cookInfoToJson(String &in, String name) {
    String *output = &in;
    DynamicJsonBuffer jsonBuffer;
    JsonObject& root = jsonBuffer.createObject();
    root["steps"] = steps;
    root["peakFreq"] = peakFreq;
    root.printTo(*output);
}

int lowFreqFilter(int inputValue, int lastFilteredValue, int alpha) {
    return (lastFilteredValue +  alpha * (inputValue - lastFilteredValue)+100);
}

void updateAccelerometer()
{
    int filteredSignal = 0;
    int sourceSignal = analogRead(A0);
    accelerometer.lastValue = map(sourceSignal, 0, 1023, graphAcc.size-1, 0);

    if (graphAcc.counter != 0) {
        filteredSignal = graphAcc.array[graphAcc.counter - 1] + 0.5 * (accelerometer.lastValue - graphAcc.array[graphAcc.counter - 1]);
    }

    if (graphAcc.counter < graphAcc.size - 1)
    {   
        graphAcc.array[graphAcc.counter] = filteredSignal;
        graphAcc.source[graphAcc.counter] = sourceSignal;
        graphAcc.counter++;
    }
    else
    {
        for (int i = 0; i != graphAcc.counter; i++)
        {
            graphAcc.array[i] = graphAcc.array[i + 1];
            graphAcc.source[i] = graphAcc.source[i + 1];
        }
        graphAcc.array[graphAcc.counter] = filteredSignal;
        graphAcc.source[graphAcc.counter] = sourceSignal;
        graphAcc.peakFreqArray[graphAcc.counter] = peakFreq;
        
        //start fft
        for (int i = 0; i < graphAcc.size; i++) {
            vReal[i] = (double) graphAcc.source[i];
            vImag[i] = 0;
        }
        FFT.Windowing(vReal, graphAcc.size, FFT_WIN_TYP_RECTANGLE, FFT_FORWARD);
        FFT.Compute(vReal, vImag, graphAcc.size, FFT_FORWARD);
        FFT.ComplexToMagnitude(vReal, vImag, graphAcc.size);
        for (int i = 0; i < graphAcc.size/2; i++) {
            graphAcc.fft[i] = map((int) vReal[i], 0, 15000, 200, 0);
        }

        
        peakFreq = FFT.MajorPeak(vReal, graphAcc.size, 30);    
        for (int i = 0; i != 127; i++) {
            graphAcc.peakFreqArray[i] = graphAcc.peakFreqArray[i + 1];
        }
        graphAcc.peakFreqArray[127] = peakFreq;
    }
}

void stepUpdater () {
    int sensorsUpdateRate = 15; // samples/sec
    double slidingWindow = 1.2; //secs
    double fftWindow = 8.5; //secs
    for (int i = 0; i < sensorsUpdateRate * fftWindow; i = i + sensorsUpdateRate * slidingWindow) {
        
        int amplCounter = 0;
        int cycleCounter = 0;
        double stepCounter = 0;
        for (int j = i; (j < (sensorsUpdateRate * slidingWindow + i)) && (j < sensorsUpdateRate * fftWindow); j++) {
            
            if ((0.7 < graphAcc.peakFreqArray[j]) && (graphAcc.peakFreqArray[j] < 2.2)) {
                amplCounter++;
                stepCounter = stepCounter + (graphAcc.peakFreqArray[j] / (sensorsUpdateRate * slidingWindow));
            }
            cycleCounter++;
        }
        if ((amplCounter / cycleCounter) > 0.7) {
            steps = steps + (int)(stepCounter);
        }
        ESP.wdtFeed();
    }
}

void handleNotFound()
{
    String message = "File Not Found\n\n";
    message += "URI: ";
    message += server.uri();
    message += "\nMethod: ";
    message += (server.method() == HTTP_GET) ? "GET" : "POST";
    message += "\nArguments: ";
    message += server.args();
    message += "\n";
    for (uint8_t i = 0; i < server.args(); i++)
        message += " " + server.argName(i) + ": " + server.arg(i) + "\n";
    server.send(404, "text/plain", message);
}

void sendAccelerometerData() {
    String word1, word2, word3;
    cookAccelerometer(word1, graphAcc.array, graphAcc.size, "data");
    cookAccelerometer(word2, graphAcc.fft, graphAcc.size/2, "fft");
    cookInfoToJson(word3, "info");
    webSocket.broadcastTXT(word1);
    webSocket.broadcastTXT(word2);
    webSocket.broadcastTXT(word3);
    webSocket.loop();
}


void webSocketEvent(uint8_t num, WStype_t type, uint8_t * payload, size_t length) {
    switch(type) {
        case WStype_TEXT:
            timeOfCal = (int) *payload;
            Serial.printf("[%u] get Text: %d\n", num, timeOfCal);
    }
}


void setup(void)
{
    IPAddress apIP(2, 2, 2, 2);
    Serial.begin(115200);
    Serial.setDebugOutput(true);
    Serial.println();
    
    WiFi.setAutoConnect(false); 
    WiFi.softAPConfig(apIP, apIP, IPAddress(255, 255, 255, 0));
    WiFi.softAP(ssid);
    IPAddress myIP = WiFi.softAPIP();
    Serial.print("AP IP address: ");
    Serial.println(myIP);

    server.on("/accelerometer", handleAccelerometer);
    server.on("/", []() {
        server.send(200, "text/html", "<a style=\"font-size:10em;2\"href=\"http://2.2.2.2/accelerometer\">Main Page</a>");
    });
    server.onNotFound(handleNotFound);

    server.begin();
    Serial.println("HTTP server started");
    
    webSocket.begin();
    webSocket.onEvent(webSocketEvent);

    pinMode(A0, INPUT);
    sensorTicker.attach_ms(33, updateAccelerometer);
    stepTicker.attach_ms(8500, stepUpdater);
}

void loop(void)
{   
    server.handleClient();
    delay(10);
    sendAccelerometerData();
    delay(10);
}