/*
*/

#include <RTSMonitoringTask.h>
#include <RTSErrResult.h>
#include <RelGILLock.h>
//#include "../../include/json.hpp"//((
//#include "../../include/json_fwd.hpp"
#include <string>
#include<iostream>
#include <memory>
#include<cstring>
#include <RTSMonFrame.h>
#include<ISensorDesc.h>




#include <chrono>
#include <sstream>
#include <fstream>
#include <VibSensorDesc.h>





// #define RPMSG_SDB_IOCTL_SET_EFD _IOW('R', 0x00, struct rpmsg_sdb_ioctl_set_efd *)
// #define RPMSG_SDB_IOCTL_GET_DATA_SIZE _IOWR('R', 0x01, struct rpmsg_sdb_ioctl_get_data_size *)
#define RPMSG_SDB_IOCTL_SET_EFD _IOW('R', 0x00, rpmsg_sdb_ioctl_set_efd *)
#define RPMSG_SDB_IOCTL_GET_DATA_SIZE _IOWR('R', 0x01, rpmsg_sdb_ioctl_get_data_size *)

#define DATA_BUF_POOL_SIZE 1024*1024 /* 1MB */
#define TIMEOUT 1 * 1000 



RTSMonitoringTask::RTSMonitoringTask(std::string id, std::string name, int samplerate, int sampleperiod, int downtime, std::string path)
{
    this->id = id;
    this->monName = name;
    this->samplerate = samplerate;
    this->sampleperiod = sampleperiod;
    this->downtime = downtime;
    this->path = path;
}




RTSMonitoringTask::~RTSMonitoringTask()
{
}

bool RTSMonitoringTask::Load() {
    //later load the file
    //int id = 1;
    return true;
}

bool RTSMonitoringTask::Init() {
    
    const std::string fwPath = "home/root/elffile";
    const std::string fwName = "zephyr_openamp_rsc_table.elf";

    //check if the FWIsRunning
    // if (coproHelper.Init(fwPath, fwName) >= 0) {
    //     printf("CA7 : Init success \n");

        if (pthread_create( &monThread, NULL, &RTSMonitoringTask::Run, this) == 0) {
            printf("CA7 : mon thread creation fails\n");
            return true;
        }
        return false;
}


void* RTSMonitoringTask::Run(void *obj) {

    RTSMonitoringTask* rtsMonTask = static_cast<RTSMonitoringTask*>(obj);

    int fdSdbRpmsg = rtsMonTask->Open("/dev/mon-datafile");
    if (fdSdbRpmsg > 0) {
        while (rtsMonTask->GetRunState()) {
            rtsMonTask->Read();            
        }
        rtsMonTask->Close();
    }
    return 0;
}

bool RTSMonitoringTask::GetRunState() {
    return mRunThread;
}


void RTSMonitoringTask::SetRunState(bool runThread) {
    this->mRunThread.store(runThread) ;
}


int RTSMonitoringTask::Open(std::string monfilename){    
    //PyRelinquishGIL scoped;

    this->mFdSdbRpmsg = open(monfilename.c_str(), O_RDWR);
    
    if (this->mFdSdbRpmsg < 0) {
        printf("CA7 : Err openening %s, ret=%d\n",monfilename.c_str(), this->mFdSdbRpmsg);  
    }
    else {
        printf("CA7 : Succes openening %s, ret=%d\n",monfilename.c_str(), this->mFdSdbRpmsg); 

        //this->log(this->format("CA7 : Success on openening %s, ret=%d\n",monfilename, this->mFdSdbRpmsg), f);                                          
        for (int i=0;i<NB_BUF;i++){
            // Create the evenfd, and sent it to kernel driver, for notification of buffer full
            this->mefd[i] = eventfd(0, 0);
            if (this->mefd[i] == -1) {
                printf("CA7 : failed to get eventfd:%d\n",i);
                //this->mLogs.push_back("CA7 : failed to get eventfd:%d");
            }
            else {
                printf("CA7 : Forward efd info for buf%d with mFdSdbRpmsg:%d and efd:%d\n",i,this->mFdSdbRpmsg,this->mefd[i]);
            }

            this->q_set_efd.bufferId = i;
            this->q_set_efd.eventfd = this->mefd[i];
            if(ioctl(mFdSdbRpmsg, RPMSG_SDB_IOCTL_SET_EFD, &(this->q_set_efd)) < 0) {
                printf("CA7 :failed to set efd efd%d-mFdSdbRpmsg%d for :%d\n",i,this->mFdSdbRpmsg,this->mefd[i]);            
            }
            else {
                printf("CA7 : succes to set efd efd%d-mFdSdbRpmsg%d:%d\n",i,this->mFdSdbRpmsg,this->mefd[i]);
            }

            // watch eventfd for input
            this->mfds[i].fd = this->mefd[i];
            this->mfds[i].events = POLLIN;
            printf("CA7 : start mapping buffer for efd:%d\n",i);
            // opensuccess.format("CA7 : start mapping buffer for efd:%d",i);                        
            // this->mLogs.push_back(this->format(u8"CA7 : start mapping buffer for efd:%d",i));
            //this->mLogs.push_back("CA7 : start mapping buffer for efd");

            mmappedData[i] = mmap(NULL,
                                    DATA_BUF_POOL_SIZE,
                                    PROT_READ | PROT_WRITE,
                                    MAP_PRIVATE,
                                    this->mFdSdbRpmsg,
                                    0);
            
            printf("CA7 : DBG mmappedData[%d]:%p\n", i, mmappedData[i]);
            this->mfMappedData = true;
            usleep(50000);
        }
    }

    //assert(this->mFdSdbRpmsg != -1);
    return this->mFdSdbRpmsg;
}


std::string RTSMonitoringTask::Read(){
    //PyRelinquishGIL scoped;
    
    std::string result;
    std::list<std::shared_ptr<ISensorDesc>> sensDescs;
    std::shared_ptr<ISensorDesc> sensorDesc = std::make_shared<VibSensorDesc>("RT850");
    sensDescs.push_back(sensorDesc);

    //std::string TEST = "Test";
    RTSMonFrame frame(this->id,this->monName, this->samplerate, this->sampleperiod, this->downtime, sensDescs, this->path);


    int ret, rc;
    char buf[16];
    char dbgmsg[80];

    // wait till at least one buffer becomes available
    //this->mLogs.push_back("CA7 : Start Polling.\n");                
    //printf("CA7 : Start Polling.\n");    
    ret = poll(this->mfds, NB_BUF, TIMEOUT); //ret = poll(fds, NB_BUF, TIMEOUT * 1000);

    if (ret == -1) {
        perror("poll()");
        //this->mLogs.push_back("CA7 : Error Polling.\n");
        printf("CA7 : Poll Error.\n");
        return std::string("poll error");                                
    }
    else if (ret == 0){
        printf("CA7 : No buffer data within %d msec.\n", TIMEOUT);
        //this->mLogs.push_back("CA7 : No buffer data within 300 msec.\n");
        return std::string("CA7 : No buffer data within %d msec.\n");                                
    }
   
    for (size_t i = 0; i < NB_BUF; i++)
    {
        int revent = this->mfds[i].revents;
        printf("CA7 : try Reading Buffer %d, reevent=%d, POLLIN=%d.\n", i, revent, POLLIN);                
        if (this->mfds[i].revents & POLLIN) {
            printf("CA7 : Start Reading Buffer %d.\n", i );        
            rc = read(this->mefd[i], buf, 16);
            if (!rc) {
                return std::string("CA7: stdin closed - Buffer:%d\n", i);
            }
            
            /* Get buffer data size*/
            this->q_get_data_size.bufferId = i;

            if(ioctl(this->mFdSdbRpmsg, RPMSG_SDB_IOCTL_GET_DATA_SIZE, &(this->q_get_data_size)) < 0) {
                printf("CA7 : Failed to get data size.\n");
                return std::string("Failed to get data size\n");
            }

            if (this->q_get_data_size.size) {
                printf("CA7 : call generate frame - datasize:%d.\n", this->q_get_data_size.size);
                mNbUncompMB++;
                mNbUncompData += this->q_get_data_size.size;

                u_int32_t* u32Buff = (u_int32_t*) this->mmappedData[i];
                frame.generate(u32Buff, this->q_get_data_size.size);
                this->mfds[i].revents = 0;
            }
            // else {
            //     result = std::string("CA7 : file could not be written.\n") + filename;                
            //     //printf("CA7 : file %s could not be written.\n", filename);                                
            // }
            //outfile.close();

                                
            // gettimeofday(&tval_after, NULL);
            // timersub(&tval_after, &tval_before, &tval_result);

            //this->mLogs.push_back("CA7: mon_thread data EVENT mDdrBuffAwaited\n");                                                                
        }
        // else {
        //     //printf("CA7 : sdb_thread => buf[%d] is empty\n", mDdrBuffAwaited);
        //     //this->mLogs.push_back("CA7: buffer is empty\n");
        //     return std::string("CA7: buffer is empty\n");                                            
        // }
        // mDdrBuffAwaited++;
        // if (mDdrBuffAwaited >= NB_BUF) {
        //     mDdrBuffAwaited = 0;
        // }
    }
    return result;
}

bool RTSMonitoringTask::Close(){
    //PyRelinquishGIL scoped;
    
    printf("CA7 : close of device \n");

    if (this->mfMappedData) {
        for (int i=0;i<NB_BUF;i++){
            int rc = munmap(mmappedData[i], DATA_BUF_POOL_SIZE);
            if (rc != 0) {
                printf("CA7 : Buffer could not be unmapped\n");                
            }
        }
        this->mfMappedData = 0;
    }

    int res = close(this->mFdSdbRpmsg);
    if (res != 0) {        
        printf("CA7 : close of device failed: %s\n", strerror(errno));        
        return false;
    }
                                    
    printf("CA7 : close of device done\n");            
    return true;
}


bool RTSMonitoringTask::Start(){
    const std::string fwPath = "home/root/elffile";
    const std::string fwName = "zephyr_openamp_rsc_table.elf";


    if (pthread_create( &monThread, NULL, &RTSMonitoringTask::Run, this) == 0) {
        printf("CA7 : mon thread creation success\n");
        return true;
    }
    printf("CA7 : mon thread creation fails\n");    
    return false;
}

bool RTSMonitoringTask::Stop(){
    this->SetRunState(false);
    printf("CA7 : mon thread terminating\n");
    pthread_join(monThread, NULL); 
    printf("CA7 : mon thread terminated\n");
    return true;
}

std::string RTSMonitoringTask::GetId(){
    return this->id;
}


void RTSMonitoringTask::SetId(std::string id){
    this->id = id;
}

int RTSMonitoringTask::GetRate(){
    return this->samplerate;
}

void RTSMonitoringTask::SetRate(int rate){
    this->samplerate = rate;
}

int RTSMonitoringTask::GetPeriod(){
    return this->sampleperiod;
}

void RTSMonitoringTask::SetPeriod(int period){
    this->sampleperiod = period;
}

int RTSMonitoringTask::GetDowntime(){
    return this->downtime;
}

void RTSMonitoringTask::SetDowntime(int downtime){
    this->downtime = downtime;
}

std::string RTSMonitoringTask::GetPath(){
    return this->path;
}

void RTSMonitoringTask::SetPath(std::string path){
    this->path = path;
}
