#include <ethercat.h>
#include <stdio.h>
#include <string.h>

#include "ecmbind.h"

int TIMEOUT_PROCESSDATA = 2000;

char ecd_iomap[4096];
int ecd_iomap_size;
int ecd_expected_wkc_size;

int ecmbind_version(char *versionString)
{
    strcpy(versionString, "ecmbind/0.1.0");
    return 0x00010000;
}

int ecmbind_initialize(char *ifname)
{
    return ec_init(ifname);
}

int ecmbind_config_init()
{
    ec_config_init(FALSE);
    return ec_slavecount;
}

void*   ecmbind_get_iomap() { return ecd_iomap; }
int     ecmbind_get_expected_wkc_size() { return ecd_expected_wkc_size; }

int     ecmbind_config_map()
{
    ecd_pdo_map_length = 0;
    
    ecd_iomap_size = ec_config_map(&ecd_iomap);
    //ec_configdc();
    ecd_expected_wkc_size = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;

    for (int slave=1;slave<=ec_slavecount;slave++)
        ecd_read_pdo_map(slave);

    return ecd_iomap_size;
}

uint16     ecmbind_read_state()
{
    return ec_readstate();
}

uint16     ecmbind_get_slave_state(int slave)
{
    return ec_slave[slave].state;
}


int     ecmbind_write_slave_state(int slave, uint16 state)
{
    ec_slave[slave].state = state;
    return ec_writestate(slave);
}

uint16  ecmbind_request_state(int slave, uint16 reqState, int timeout)
{
    return ec_statecheck(slave, reqState, timeout * 1000);
}

int    ecmbind_processdata()
{
    ec_send_processdata();
    int wkc = ec_receive_processdata(EC_TIMEOUTRET);
    return wkc;
}

int    ecmbind_processdata2(char *managed_iomap, int size)
{
    if (size != ecd_iomap_size)
        return -1;

    memcpy( ecd_iomap, managed_iomap, ecd_iomap_size);

    ec_send_processdata();
    int wkc = ec_receive_processdata(EC_TIMEOUTRET);

    memcpy( managed_iomap, ecd_iomap, ecd_iomap_size);

    return wkc;
}

int     ecmbind_recover()
{
    int slave = 0;
    ec_group[0].docheckstate = FALSE;
    ec_readstate();
    for (slave = 1; slave <= ec_slavecount; slave++)
    {
        if ((ec_slave[slave].group == 0) && (ec_slave[slave].state != EC_STATE_OPERATIONAL))
        {
            ec_group[0].docheckstate = TRUE;
            if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR))
            {
                printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave);
                ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK);
                ec_writestate(slave);
            }
            else if(ec_slave[slave].state == EC_STATE_SAFE_OP)
            {
                printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave);
                ec_slave[slave].state = EC_STATE_OPERATIONAL;
                ec_writestate(slave);
            }
            else if(ec_slave[slave].state > EC_STATE_NONE)
            {
                if (ec_reconfig_slave(slave, TIMEOUT_PROCESSDATA))
                {
                ec_slave[slave].islost = FALSE;
                printf("MESSAGE : slave %d reconfigured\n",slave);
                }
            }
            else if(!ec_slave[slave].islost)
            {
                /* re-check state */
                ec_statecheck(slave, EC_STATE_OPERATIONAL, TIMEOUT_PROCESSDATA);
                if (ec_slave[slave].state == EC_STATE_NONE)
                {
                ec_slave[slave].islost = TRUE;
                printf("ERROR : slave %d lost\n",slave);
                }
            }
        }

        if (ec_slave[slave].islost)
        {
            if(ec_slave[slave].state == EC_STATE_NONE)
            {
                if (ec_recover_slave(slave, TIMEOUT_PROCESSDATA))
                {
                    ec_slave[slave].islost = FALSE;
                    printf("MESSAGE : slave %d recovered\n",slave);
                }
            }
            else
            {
                ec_slave[slave].islost = FALSE;
                printf("MESSAGE : slave %d found\n",slave);
            }
        }
    }

    if(!ec_group[0].docheckstate)
    {
        printf("OK : all slaves resumed OPERATIONAL.\n");
        ecd_expected_wkc_size = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
        return ecd_expected_wkc_size;
    }
    return -1;
}

int ecmbind_get_pdo_entries_length() { return ecd_pdo_map_length; }
int ecmbind_get_pdo_entries(ecd_pdo_entry_t* table, int length)
{
    if (length < ecd_pdo_map_length)
        return -1;

    int tableSize = (sizeof(ecd_pdo_entry_t)* ecd_pdo_map_length);
    memcpy(table, ecd_pdo_map, tableSize);

    return ecd_pdo_map_length;
}

int ecmbind_pdo_read(int slave, int index, int subindex, char* buffer, int size)
{
    return -1;
}
int ecmbind_pdo_write(int slave, int index, int subindex, char* buffer, int size)
{
    return -1;
}

int ecmbind_sdo_read(int slave, int index, int subindex, char* buffer, int size)
{
    int wkc = ec_SDOread(slave, index, subindex, FALSE, &size, buffer, 100000);
    if (wkc <= 0)
        return -1;
    return size;
}
int ecmbind_sdo_write(int slave, int index, int subindex, char* buffer, int size)
{
    return -1;
}

/*
typedef void (*cb_enum_pdo)(int slave, int index, int subindex, int addr_offset, int addr_bit, int bitlength);
*/
int ecmbind_pdo_enumerate(cb_enum_pdo cb)
{
    printf("libecmbind: ecmbind_pdo_enumerate(): enumerating %d PDOs\n", ecd_pdo_map_length);
    for (int n=0;n<ecd_pdo_map_length;n++)
    {
        cb(ecd_pdo_map[n].slave, ecd_pdo_map[n].index, ecd_pdo_map[n].subindex, ecd_pdo_map[n].addr_offset, ecd_pdo_map[n].addr_bit, ecd_pdo_map[n].bitlength);
    }
}

int ecmbind_iomap_get(int offset, char* buffer, int length)
{
    if (offset + length > ecd_iomap_size)
        return -1;
    memcpy(buffer, &ecd_iomap[offset], length);
    return length;
}
int ecmbind_iomap_set(int offset, char* buffer, int length)
{
    if (offset + length > ecd_iomap_size)
        return -1;

    memcpy(&ecd_iomap[offset], buffer, length);
}