#include "Program.h"
#include "Arduino.h"
#include "M5LCD.h"
#include "DolibarrClient.h"
#include "ServoMotorComponent.h"
#include "NfcReader.h"
#include <Ticker.h>

uint32_t derniereExecution = 0;
const uint32_t intervalle = 1000;
std::vector<models::Warehouse> *warehouses;

int initialize_wifi(WifiConfig wifi) {
    lcdScreen->add_log("Connecting to the WiFi network...");
    WiFiClass::mode(WIFI_STA); //Optional
    WiFi.setSleep(false);
    WiFi.begin(wifi.ssid, wifi.password);
    return 0;
}

void Program::checkNfc() {
    if (this->nfcReader->IsNfcConnected() && lcdScreen->get_components().nfc != COMPONENT_OK) {
        lcdScreen->set_nfc_status(COMPONENT_OK);
        lcdScreen->add_log("NFC component connected !");
        Serial.println("NFC IS CONNECTED !");
    } else if (!this->nfcReader->IsNfcConnected() && lcdScreen->get_components().nfc == COMPONENT_OK) {
        lcdScreen->set_nfc_status(COMPONENT_KO);
        lcdScreen->add_log("NFC component disconnected !");
        Serial.println("NFC NOT CONNECTED !");
    }
}

void Program::checkServo() {
    if (this->servo->isConnected() && lcdScreen->get_components().servo != COMPONENT_OK) {
        lcdScreen->set_servo(COMPONENT_OK);
        lcdScreen->add_log("SERVO component connected !");
        Serial.println("SERVO IS CONNECTED !");
    } else if (!this->servo->isConnected() && lcdScreen->get_components().servo == COMPONENT_OK) {
        lcdScreen->set_servo(COMPONENT_KO);
        lcdScreen->add_log("SERVO component disconnected !");
        Serial.println("SERVO NOT CONNECTED !");
    }
}

void Program::checkWifi() {
    if (WiFiClass::status() == WL_CONNECTED && lcdScreen->get_components().wifi != COMPONENT_OK) {
        lcdScreen->add_log("Connected to the WiFi network");
        lcdScreen->set_wifi_status(COMPONENT_OK);
        struct DolibarrConfig dolibarr = {DOLIBARR_URL, DOLIBARR_API_TOKEN};
        this->client = new DolibarrClient(dolibarr);
        warehouses = this->client->list_warehouse();
        if (warehouses != nullptr) {
            lcdScreen->add_log("Warehouses found !");
            lcdScreen->set_dolibarr_status(COMPONENT_OK);
            for (auto &ware : *warehouses) {
                char buffer[50];
                sprintf(buffer, "+ Warehouse '%s' (%s)", ware.label.c_str(), ware.id.c_str());
                lcdScreen->add_log(buffer);
            }
        } else {
            lcdScreen->add_log("Warehouse not found");
            lcdScreen->set_dolibarr_status(COMPONENT_KO);
        }
    } else if (WiFiClass::status() != WL_CONNECTED && lcdScreen->get_components().wifi == COMPONENT_OK) {
        lcdScreen->add_log("Wifi signal lost, reconnecting...");
        lcdScreen->set_wifi_status(COMPONENT_KO);
        lcdScreen->set_dolibarr_status(COMPONENT_KO);
        struct WifiConfig wifi_c = {WIFI_SSID, WIFI_PASSWORD};
        initialize_wifi(wifi_c);
    }
}

Program::Program() {
    Serial.begin(MONITOR_SPEED);
    Wire.begin();
    delay(1000);
    lcdScreen = new M5LCD();
    lcdScreen->add_log("Initialize M5LCD component....");
    this->nfcReader = new NfcReader(NFC_ADDR);
    this->servo = new ServoMotorComponent(2, 1, 1);
    this->servo->setDesiredPosition(Position::MIDDLE);
    this->grbl = new GRBL(STEPMOTOR_I2C_ADDR);
    this->outputReader = new BigNfcReader();
    this->outputReader->init();
    this->grblUpdateTime = 0;
    lcdScreen->set_grbl_status(COMPONENT_OK);
    Wire.begin(21, 22);
    grbl->init(STEPER_SPEED, STEPER_PAS, STEPER_ACC);
    struct WifiConfig wifi_c = {WIFI_SSID, WIFI_PASSWORD};
    initialize_wifi(wifi_c);
}

void Program::loop() {
    lcdScreen->update();

    uint32_t maintenant = millis();
    if (maintenant - derniereExecution >= intervalle) {
        this->checkServo();
        this->checkNfc();
        this->checkWifi();
        derniereExecution = maintenant;
    }
    this->servo->refresh();
    this->outputReader->refresh();
    // Serial.println(this->outputReader->getNbTags());

    if(this->outputReader->getNbTags() >= 2){
        lcdScreen->set_nfc_message("To mutch colis number detected");
        lcdScreen->set_grbl_status(COMPONENT_KO);
    }

    String nfcId = this->nfcReader->ReadNfc();
    //si qqc
    if(nfcId != "0"){
        if (lcdScreen->get_components().wifi != COMPONENT_OK) {
            lcdScreen->add_log("Wifi not connected !");
            lcdScreen->set_nfc_message("Cannot send wifi request !");
            return;
        }
        //j'arrète le stepper
        //this->grbl->stop(); //FIXME: implemente
        //j'affiche le tag lue
        Serial.print("new colis in comming : ");
        lcdScreen->add_log("new colis detected: ");
        lcdScreen->add_log(nfcId.c_str());
        lcdScreen->set_nfc_message(nfcId.c_str());
        auto product = this->client->get_product_by_factory_id(nfcId.c_str());
        Serial.printf("Product: %s\n%s", product.label.c_str(), product.accountancy_code_sell_export.c_str());
        lcdScreen->set_product_label(product.label);
        lcdScreen->set_product_id(product.id);
        /*if (product.fk_default_warehouse == product.accountancy_code_sell_export) {
            Serial.printf("Product already in the good warehouse !\n");
            return;
        }*/
        auto ware = std::find_if(warehouses->begin(), warehouses->end(), [&product](models::Warehouse w) {return w.id == product.accountancy_code_sell_export;});
        if (ware.base() == nullptr) {
            Serial.printf("Warehouse not found !\n");
            return;
        }
        Serial.printf("Product need to go to warehouse %s\n", ware->label.c_str());
        this->client->create_movement(models::CreateProductStock{
                    product.id,
                    product.fk_default_warehouse,
                    "-1"
        });
        if (this->client->create_movement(models::CreateProductStock{
                product.id,
                ware->id,
                "1"
            }) == 0) {
            Serial.printf("Movement created !\n");
        } else {
            Serial.printf("Movement cannot be created !\n");
        }
        if (ware->id == "1") {
            this->servo->setDesiredPosition(Position::RIGHT);
            lcdScreen->set_servo_message("RIGHT");
        } else if (ware->id == "2") {
            this->servo->setDesiredPosition(Position::LEFT);
            lcdScreen->set_servo_message("LEFT");
        } else if (ware->id == "3") {
            this->servo->setDesiredPosition(Position::MIDDLE);
            lcdScreen->set_servo_message("MIDDLE");
        }
        this->grbl->mouveForward(CONVOYER_LEN);
    } else {
        if((this->grbl->isIddle() || (maintenant - this->grblUpdateTime >= GRBL_UPDATE)) && lcdScreen->get_components().grbl == COMPONENT_OK){
            this->grblUpdateTime = maintenant;
            this->grbl->mouveForward(5);
        }
    }
    if(M5.BtnC.wasReleased() != 0 && lcdScreen->_current_page == DASHBOARD_SCREEN){
        lcdScreen->set_grbl_status(COMPONENT_OK);
    }
}