#include "worker.h"

#include "ecrt.h"
#include <sys/mman.h>

#include <QThread>
#include <QDebug>

// outputs offsets
static unsigned int off_TargetPosition;
static unsigned int off_TargetVelocity;
static unsigned int off_TargetTorque;
static unsigned int off_MaxTorque;
static unsigned int off_ctl_word;
static unsigned int off_ModeOfOperation;
static unsigned int off_DigitalOutput;

// inputs offsets
static unsigned int off_position_value;
static unsigned int off_DigitalInputs;
static unsigned int off_status;



static ec_domain_state_t domain1_state = {};
static ec_master_state_t master_state = {};



const static ec_pdo_entry_reg_t domain1_regs[] = {
    /* outputs */
    {0, 0, 0x0000009A, 0x00030924, 0x607a, 0, &off_TargetPosition},
    {0, 0, 0x0000009A, 0x00030924, 0x60ff, 0, &off_TargetVelocity},
    {0, 0, 0x0000009A, 0x00030924, 0x6071, 0, &off_TargetTorque},
    {0, 0, 0x0000009A, 0x00030924, 0x6072, 0, &off_MaxTorque},
    {0, 0, 0x0000009A, 0x00030924, 0x6040, 0, &off_ctl_word},
    {0, 0, 0x0000009A, 0x00030924, 0x6060, 0, &off_ModeOfOperation},

    {0, 0, 0x0000009A, 0x00030924, 0x60fe, 1, &off_DigitalOutput},

    /* inputs */
    {0, 0, 0x0000009A, 0x00030924, 0x6064, 0, &off_position_value},
    {0, 0, 0x0000009A, 0x00030924, 0x60fd, 0, &off_DigitalInputs},
    {0, 0, 0x0000009A, 0x00030924, 0x6041, 0, &off_status},
    {}
};

ec_pdo_entry_info_t slave_0_pdo_entries[] = {
    /* RxPdo 0x1605 */
    {0x607a, 0x00, 32},     /* target position */
    {0x60ff, 0x00, 32},     /* target velocity */
    {0x6071, 0x00, 16},     /* target torque */
    {0x6072, 0x00, 16},     /* max. torque */
    {0x6040, 0x00, 16},     /* control word */
    {0x6060, 0x00, 8},      /* mode of operation */

    /* RxPdo 0x161d */
    {0x60fe, 0x01, 32},     /* digital outputs */

    /* TxPdo 0x1a00 */
    {0x6064, 0x00, 32},     /* position actual value */
    {0x60fd, 0x00, 32},     /* digital inputs */
    {0x6041, 0x00, 16},     /* status word */
};

ec_pdo_info_t slave_0_pdos[] = {
    {0x1605, 6, slave_0_pdo_entries + 0},
    {0x161d, 1, slave_0_pdo_entries + 6},
    {0x1a00, 3, slave_0_pdo_entries + 7},
};

ec_sync_info_t slave_0_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 2, slave_0_pdos + 0, EC_WD_DISABLE},
    {3, EC_DIR_INPUT, 1, slave_0_pdos + 2, EC_WD_DISABLE},
    {0xff}
};


static ec_master_t *master = nullptr;
static ec_domain_t *domain1 = nullptr;
static uint8_t *domain1_pd = nullptr;
static ec_slave_config_t *sc_elmo = nullptr;
static ec_slave_info_t slaveInfo;

Worker::Worker(QObject *parent) : QObject(parent),
    lControlWord(0),
    lControlWordBackup(0),
    lStatus(0),
    lStatusBackup(0),
    initStep(0),
    lPositionValue(0),
    lPositionValueBackup(0),
    lDigitalInputsValue(0),
    lDigitalInputsValueBackup(0),
    lDigitalOutputsValue(0)
{

}

Worker::~Worker()
{
  if(initStep > 0)
      ecrt_release_master(master);
}

bool Worker::init()
{

    uint8_t lHomingMethod[] = {33};
//    uint8_t lModeOfOperation[] = {6};     // homing mode
//    uint8_t lModeOfOperation[] = {3};     // velocity mode
    uint8_t lModeOfOperation[] = {1};     // position mode

    uint8_t target[] = {0};
    size_t result_size;
    uint32_t abort_code = 0;

    if(initStep == 0)
    {
        master = ecrt_request_master(0);
        if ( !master ) {
            return false;
        }
        ++initStep;
    }

    if(initStep == 1)
    {
        domain1 = ecrt_master_create_domain(master);
        if ( !domain1 ) {
            return false;
        }
        ++initStep;
    }


    if(initStep == 2)
    {
        if ( !(sc_elmo = ecrt_master_slave_config(master, 0, 0, 0x0000009A, 0x00030924)) ) {
            fprintf(stderr, "Failed to get slave configuration for AKD.\n");
            return false;
        }
        ++initStep;
    }

    if(initStep == 3)
    {
        if(ecrt_master_sdo_download(master, 0, 0x6098, 0, (const uint8_t*)lHomingMethod, sizeof(lHomingMethod), &abort_code)<0)
        {
            return false;
        }
        ++initStep;
    }

    if(initStep == 4)
    {

        if(ecrt_master_sdo_download(master, 0, 0x6060, 0, (const uint8_t*)lModeOfOperation, sizeof(lModeOfOperation), &abort_code)<0)
        {
            return false;
        }
        ++initStep;
    }


    if(initStep == 5)
    {
        if (ecrt_slave_config_pdos(sc_elmo, EC_END, slave_0_syncs))
        {
            return false;
        }
        ++initStep;
    }

    if(initStep == 6)
    {
        if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs))
        {
            return false;
        }
        ++initStep;
    }

    if(initStep == 7)
    {
        if ( ecrt_master_activate( master ) )
        {
            return false;
        }
        ++initStep;
    }

    if(initStep == 8)
    {
        if (!(domain1_pd = ecrt_domain_data(domain1)))
        {
            return false;
        }
        ++initStep;
    }

    return true;
}

void Worker::stop()
{
    mMustStop = true;
}

void Worker::setControWord(uint16_t cw)
{
    lControlWord = cw;
}

void Worker::setDigitalOutputs(uint32_t digitalOutputs)
{
    lDigitalOutputsValue = digitalOutputs;
}

void Worker::check_domain1_state()
{
    ec_domain_state_t ds;

    ecrt_domain_state(domain1, &ds);

    if (ds.working_counter != domain1_state.working_counter) {
        printf("Domain1: WC %u.\n", ds.working_counter);
    }
    if (ds.wc_state != domain1_state.wc_state) {
        printf("Domain1: State %u.\n", ds.wc_state);
    }

    domain1_state = ds;
}

void Worker::loop()
{
    mMustStop = false;

    while(!mMustStop)
    {
//        QThread::msleep(100);
        QThread::usleep(1000);

        // receive process data
        ecrt_master_receive(master);
        ecrt_domain_process(domain1);

        // check process data state
        check_domain1_state();

        lStatus = EC_READ_U16(domain1_pd + off_status);

        if(lStatus != lStatusBackup)
        {
            emit statusChanged(lStatus);
            lStatusBackup = lStatus;
        }

        lPositionValue = EC_READ_U32(domain1_pd + off_position_value);

        if(lPositionValue != lPositionValueBackup)
        {
            emit positionValueChanged(lPositionValue);
            lPositionValueBackup = lPositionValue;
        }

        lDigitalInputsValue = EC_READ_U32(domain1_pd + off_DigitalInputs);

        if(lDigitalInputsValue != lDigitalInputsValueBackup)
        {
            emit digitalInputsValueChanged(lDigitalInputsValue);
            lDigitalInputsValueBackup = lDigitalInputsValue;
        }

        //lControlWord = EC_READ_U16(domain1_pd + off_ctl_word);
        if(lControlWord != lControlWordBackup)
        {
            lControlWordBackup = lControlWord;
            emit controlWordChanged(lControlWordBackup);
        }     

        EC_WRITE_U16(domain1_pd + off_ctl_word, lControlWordBackup);

        EC_WRITE_U32(domain1_pd + off_DigitalOutput, lDigitalOutputsValue);

        ecrt_domain_queue(domain1);
        ecrt_master_send(master);
    }

    emit finished();
}
