#include "EepromParams.h"
#include <modules/my_task_creator/TaskCreator.h>

void EepromParams::TaskEeprom(void *pvParameters)
{
    log_i("Task started");
    TaskCreator *mCreator = (TaskCreator *)pvParameters;
    while (1)
    {
        uint16_t i = 0;
        if (mCreator->mEeprom.GetIdxFromQueue(i))
        {
            float p = mCreator->mParams.GetValue(i);
            mCreator->mEeprom.WriteToEeeprom(i, p);
            
            mCreator->mEeprom.CalcAndWriteCrc();
            log_v("Eeprom write:%i %f\n\r",i,p);
            if(mCreator->mParams.GetName(i)=="CTRL_REBOOT")
            {
                xTaskCreatePinnedToCore(
                TaskReboot
                ,"TaskReboot"
                ,2048  // Stack size
                ,NULL
                ,3  // Priority
                ,NULL 
                ,ARDUINO_RUNNING_CORE
                );
            }
        }
        vTaskDelay(100);
    }
}

void EepromParams::TaskReboot(void *pvParameters)
{
    log_i("Task started");
    vTaskDelay(2000);
    ESP.restart();
}

void EepromParams::Init(std::vector<float> p)
{
    uint16_t size = p.size();
    EEPROM_SIZE = size * sizeof(float) + 1 + sizeof (uint64_t);
    
    r2d2 = ESP.getEfuseMac();

    
    if (!EEPROM.begin(EEPROM_SIZE))
    {

        log_i("CTRL: EEPROM init failed");
    }
    else
    {

        log_i("CTRL: EEPROM is inited with %d bytes size", (uint16_t)EEPROM_SIZE);
        
        EEPROM.readBytes(0, (void*) &c3po, sizeof(uint64_t));

        if (((c3po&0xFFFFFFFF) == 0xFFFFFFFF)
            &&((c3po>>32) == 0xFFFFFFFF))
        {
            c3po = r2d2;
            c3po ^= 0xA9A9A9A9A9A9A9A9;
            EEPROM.writeBytes(0, (void*) &c3po, sizeof(uint64_t));
            EEPROM.commit();
            vTaskDelay(100);
            log_i("CTRL: magic_updated");
        } else {
            r2d2 ^= 0xA9A9A9A9A9A9A9A9;
            if ((c3po - r2d2) == 0){
                log_i("CTRL: magic_ok");
            } else {
                log_i("CTRL: magic_fails");
                            
                xTaskCreatePinnedToCore(
                TaskReboot
                ,"TaskReboot"
                ,2048  // Stack size
                ,NULL
                ,3  // Priority
                ,NULL 
                ,ARDUINO_RUNNING_CORE
                );
                return;
            }
        }
        
        mCrc=0xFF;

        for (int i = sizeof(uint64_t); i < EEPROM_SIZE - 1; i++)
        {
            uint8_t p1 = EEPROM.readByte(i);
            mCrc = crc8(mCrc,&p1,1);
        }
        
        uint8_t crc = EEPROM.readByte(EEPROM_SIZE - 1);

        if (crc != mCrc)
        {
            for (int i = 0; i < size; i++)
            {
                EEPROM.writeFloat(i * sizeof(float) + sizeof(uint64_t), p[i]);
            }
            EEPROM.commit();
            mParams = p;
            vTaskDelay(100);
            CalcAndWriteCrc();
            for (int i = 0; i < size; i++)
            {
                float par = EEPROM.readFloat(i * sizeof(float) + sizeof(uint64_t));
                mParams.push_back(par);
            }
            log_i("CTRL: EEPROM is inited default params");
            return;
        }

        for (int i = 0; i < size; i++)
        {
            float par = EEPROM.readFloat(i * sizeof(float) + sizeof(uint64_t));
            mParams.push_back(par);
        }

        log_i("CTRL: Params readed from eeprom");
    }
}


void EepromParams::WriteToEeeprom(uint16_t idx, float val)
{
    size_t n=EEPROM.writeFloat(idx * sizeof(float) + sizeof(uint64_t), val);
    if(n!=sizeof(float))
    {
        log_i("Error write eeprom size %i",n);
    }

    if(!EEPROM.commit())
    {
        log_i("Error commit in  WriteToEeeprom");
    }
    vTaskDelay(100);
}
void EepromParams::CalcAndWriteCrc()
{
    mCrc=0xFF;
    for (int i = sizeof(uint64_t); i < EEPROM_SIZE - 1; i++)
    {
        uint8_t p = EEPROM.readByte(i);
        mCrc = crc8(mCrc,&p,1);
    }

    size_t n=EEPROM.writeByte(EEPROM_SIZE - 1, mCrc);
    if(n!=1)
    {
        log_i("Error write crc");
    }
    if(!EEPROM.commit())
    {
        log_i("Error commit in  CalcAndWriteCrc");
    }
    vTaskDelay(100);
}
void EepromParams::WriteParam(uint16_t idx)
{
    mMutex.Lock();
    mWriteQueue.push(idx);
    mMutex.Unlock();
}
void EepromParams::WriteParam(uint16_t idx,float val)
{
    size_t n=EEPROM.writeFloat(idx * sizeof(float) + sizeof(uint64_t), val);
    if(n!=sizeof(float))
    {
        log_i("Error write eeprom size in WriteParam %i",n);
    }
    if(!EEPROM.commit())
    {
        log_i("Error commit in  WriteParam");
    }
    vTaskDelay(100);
    CalcAndWriteCrc();
}
bool EepromParams::GetIdxFromQueue(uint16_t &idx)
{
    bool res = false;
    mMutex.Lock();
    if (!mWriteQueue.empty())
    {
        idx = mWriteQueue.front();
        mWriteQueue.pop();
        res = true;
    }
    mMutex.Unlock();
    return res;
}

unsigned char EepromParams::crc8(unsigned char crc, unsigned char * dat, unsigned int len)
{

    while(len--)
    {
        crc ^= *dat++;

        for (unsigned int i = 0; i < 8; i++) crc = crc & 0x80 ? (crc << 1) ^ 0x31 : crc << 1;
    }

    return crc;
}