#include <Vector.h>
#include <Streaming.h>
#include <Arduino.h>

#include "app.h"
#include "features.h"


#include "H7PidController.h"

#include "common/error_codes.h"
#include "Communication.h"

static Addon *addonsArray[10];

short App::ok()
{
    return E_OK;
}

App::App() : Addon("APP", APP, 1 << STATE),
#ifdef HAS_TC
             mTC(new TemperatureController(modbusBridge)),
#endif
#ifdef HAS_COMMUNICATION
             com(new Communication())
#endif
{
    
}

short App::getAppState(short val)
{
    return _state;
}

short App::setAppState(short newState)
{
    return E_OK;
}

void printMem()
{
    Serial.print("mem: ");
    //Serial.println(freeMemory());
}
short App::setup()
{
    Serial.begin(DEBUG_BAUD_RATE);
    Serial.println("Booting Firmware ...................... ");
    addons.setStorage(addonsArray);
    setup_addons();
    printMem();
    digitalWrite(STATUS_POWER_PIN, HIGH);
    /*

    Serial.print(VERSION);
    Serial.print("\n SUPPORT :");
    Serial.print(SUPPORT);
    Serial.print("\n | BUILD: ");
    Serial.print(BUILD);
    Serial.print("\n | FIRMWARE SOURCE: ");
    Serial.println(FW_SRC);*/

    /*
    powerSwitch->on(0);
    powerSwitch->on(1);
    delay(4000);
    dFC.initFC();
    */

#ifdef MEARSURE_PERFORMANCE
    printPerfTS = 0;
    addonLoopTime = 0;
    bridgeLoopTime = 0;
#endif
    debugTS = 0;
    comTS = 0;
    loopTS = 0;
    _state = 0;
    _cstate = E_CS_OK;
    /*
    timer.every(5000, [](App *app) -> void {
        printMem();
    },
                this);
                */
    return E_OK;
}

void App::debug_mode_loop()
{
    uchar s = addons.size();
    for (uchar i = 0; i < s; i++)
    {
        Addon *addon = addons[i];
        if (addon->hasFlag(LOOP))
        {
            addon->loop();
        }
    }
}

short App::loop()
{
    loop_addons();
    timer.tick();
    now = millis();
    short error = ok();
    if (error)
    {
        _error = error;
        return E_OK;
    }
    return E_OK;    
}