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

#define ECM2_EVENT_MASK     (ECM_EVENT_LOCAL | ECM_EVENT_CFG          |   \
                            ECM_EVENT_WCNT   | ECM_EVENT_STATE_CHANGE |   \
                            ECM_EVENT_SLV)

/* Reference to process variables */
static uint16_t *pCw = NULL; //control word
static uint32_t *pTp = NULL; //target position
static uint8_t *pMop = NULL; //mode of operation
static uint32_t *pPv = NULL; //profile velocity
static uint32_t *pPa = NULL; //profile acceleration
static uint32_t *pPd = NULL; //profile deceleration
static uint16_t *pSw = NULL; //status word
static uint32_t *pPav = NULL; //position actual value

static int ecmEventHandler(ECM_EVENT *pEvent)
{
    switch(pEvent->ulEvent)
    {
        case ECM_EVENT_STATE_CHANGE:
            printf("ECM_EVENT_STATE_CHANGE: Master state change to ");
            switch(pEvent->ulArg1)
            {
            case ECM_DEVICE_STATE_UNKNOWN:
                printf("UNKNOWN\n");
                break;
            case ECM_DEVICE_STATE_INIT:
                printf("INIT\n");
                break;
            case ECM_DEVICE_STATE_PREOP:
                printf("PREOP\n");
                break;
            case ECM_DEVICE_STATE_SAFEOP:
                printf("SAFEOP\n");
                break;
            case ECM_DEVICE_STATE_OP:
                printf("OP\n");
                break;
            default:
                printf("???\n");
                break;
            }
            break;
        
        case ECM_EVENT_SLV:
            printf("ECM_EVENT_SLV: Slave address %d state change to ", pEvent->ulArg2 & 0x0000FFFF);
            switch(pEvent->ulArg1)
            {
            case ECM_ESC_AL_STATUS_INIT:
                printf("INIT\n");
                break;
            case ECM_ESC_AL_STATUS_PREOP:
                printf("PREOP\n");
                break;
            case ECM_ESC_AL_STATUS_BOOTSTRAP:
                printf("BOOTSTRAP\n");
                break;
            case ECM_ESC_AL_STATUS_SAFEOP:
                printf("SAFEOP\n");
                break;
            case ECM_ESC_AL_STATUS_OP:
                printf("OP\n");
                break;
            default:
                printf("???\n");
                break;
            }
            break;
    }
    return(0);
}

static int CycleHandler(){

}

int main(int argc, char *argv[])
{
    int result;
    ECM_LIB_INIT libInitData;
    ECM_CFG_INIT cfgInitData;
    ECM_HANDLE hndDevice =NULL;
    ECM_HANDLE hndMaster =NULL;
    ECM_HANDLE hndSlave =NULL;
    ECM_VAR_DESC VarDesc;
    ECM_PROC_CTRL ProcCtrl;
    ECM_COPY_VECTOR *pCopyVectorOutput;
    uint32_t ulEntries;
    ECM_COPY_VECTOR *pCopyVectorInput;

    ECM_INIT(libInitData);
    libInitData.ulEventMask = ECM2_EVENT_MASK;
    libInitData.pfnEventHandler = ecmEventHandler;
    result = ecmInitLibrary(&libInitData);
    if (result != 0) {
      printf("ecmInitLibrary() returned with %d\n", result);
      return(-1);
    }

    ECM_INIT(cfgInitData);
    cfgInitData.Config.File.pszEniFile = "./eni_pp_ms-mini3e.xml";
    cfgInitData.ulFlags = ECM_FLAG_CFG_KEEP_PROCVARS;
    result = ecmReadConfiguration(&cfgInitData, &hndDevice, &hndMaster);
    if (result != ECM_SUCCESS) {
        printf("Reading configuration failed with %s\n", ecmResultToString(result));
        ecmInitLibrary(NULL);
        return(result);
    }

    result = ecmGetDataReference(hndMaster, ECM_OUTPUT_DATA, 39, 2, (void **)&pCw);
    if (result != ECM_SUCCESS) {
        printf("Failed to get reference to virtual variable Output\n");
    }    

    result = ecmGetDataReference(hndMaster, ECM_OUTPUT_DATA, 41, 4, (void **)&pTp);
    if (result != ECM_SUCCESS) {
        printf("Failed to get reference to virtual variable Output\n");
    }    

    result = ecmGetDataReference(hndMaster, ECM_OUTPUT_DATA, 45, 1, (void **)&pMop);
    if (result != ECM_SUCCESS) {
        printf("Failed to get reference to virtual variable Output\n");
    }    

    result = ecmGetDataReference(hndMaster, ECM_OUTPUT_DATA, 46, 4, (void **)&pPv);
    if (result != ECM_SUCCESS) {
        printf("Failed to get reference to virtual variable Output\n");
    }

    result = ecmGetDataReference(hndMaster, ECM_OUTPUT_DATA, 50, 4, (void **)&pPa);
    if (result != ECM_SUCCESS) {
        printf("Failed to get reference to virtual variable Output\n");
    }

    result = ecmGetDataReference(hndMaster, ECM_OUTPUT_DATA, 54, 4, (void **)&pPd);
    if (result != ECM_SUCCESS) {
        printf("Failed to get reference to virtual variable Output\n");
    }

    result = ecmGetDataReference(hndMaster, ECM_INPUT_DATA, 39, 2, (void **)&pSw);
    if (result != ECM_SUCCESS) {
        printf("Failed to get reference to virtual variable Output\n");
    }    

    result = ecmGetDataReference(hndMaster, ECM_INPUT_DATA, 41, 4, (void **)&pPav);
    if (result != ECM_SUCCESS) {
        printf("Failed to get reference to virtual variable Output\n");
    }    

    result = ecmAttachMaster(hndMaster);
    if (result != ECM_SUCCESS) {
        printf("Attaching master failed with %s\n", ecmResultToString(result));
        ecmDeleteMaster(hndMaster);
        ecmDeleteDevice(hndDevice);
        ecmInitLibrary(NULL);
        return(-1);
    }

    ecmSleep(1000);

    ECM_INIT(ProcCtrl);
    ProcCtrl.ulAcyclicPeriod = 1000;
    ProcCtrl.ulAcyclicPrio   = ECM_PRIO_DEFAULT;
    ProcCtrl.ulCyclicPeriod = cfgInitData.cfgDataDevice.ulCycleTime;
    ProcCtrl.ulCyclicPrio   = ECM_PRIO_DEFAULT;
    // ProcCtrl.pfnHandler     = CycleHandler;
    result = ecmProcessControl(hndDevice, &ProcCtrl);
    if (result != ECM_SUCCESS) {
        printf("Creating background worker threads failed with %s\n", ecmResultToString(result));
        ecmDetachMaster(hndMaster);
        ecmDeleteMaster(hndMaster);
        ecmDeleteDevice(hndDevice);
        ecmInitLibrary(NULL);
        return(-1);
    } else {
        printf("Worker thread cycle times (microseconds) (Cyclic: %d)\n", ProcCtrl.ulCyclicPeriod);
    }

    result = ecmRequestState(hndMaster, ECM_DEVICE_STATE_OP, 60000);
    if (result != ECM_SUCCESS) {
        printf("Failed with %s to change network state into state %d !\n", ecmResultToString(result), ECM_DEVICE_STATE_OP);
        ecmDetachMaster(hndMaster);
        ecmDeleteMaster(hndMaster);
        ecmDeleteDevice(hndDevice);
        ecmInitLibrary(NULL);
        return(-1);
    }

    ecmSleep(1000);
    printf("Current position value is %d.\n", *pPav);
    *pMop = 1; //pp mode
    *pTp = 1000000; //target postion 1,000,000
    *pPv = 0x0000f000; //profile velocity
    *pPa = 0x00004e20; //profile acceleration
    *pPd = 0x00004e20; //profile deceleration
    *pCw = 0x003f; //absolute position, set-point updated, change immediately
    ecmSleep(1000);
    *pCw = 0x000f; 
    printf("Target position set to absolute %d.\n", *pTp);

    while (1)
    {
        if((*pSw) & 0x0400) //target reached.
        {   
            ecmSleep(3000);
            printf("Target reached, position actual value is %d.\n", *pPav);
            *pTp = -5000; 
            *pCw = 0x001f; //absolute position, set-point updated
            ecmSleep(1000);
            *pCw = 0x000f;
            printf("Target position set to absolute %d.\n", *pTp);
            while (1)
            {
                if ((*pSw) & 0x0400) 
                {
                    ecmSleep(3000);
                    printf("Target reached, position actual value is %d.\n", *pPav);
                    // *pCw = 0; 
                    // ecmSleep(1000);
                    break;
                }
            }
            break;
        }
    }

    *pTp = 2000000;
    *pCw = 0x003f; //absolute position, set-point updated, change immediately
    ecmSleep(1000);
    *pCw = 0x000f; 
    printf("Target position set to absolute %d.\n", *pTp);
    ecmSleep(5000);
    //During motor rotates, update the position. 
    //Instead of waiting target reached, change to the new position immediately.
    printf("Current position value is %d. Target NOT reached!\n", *pPav);
    *pTp = -100000;
    *pCw = 0x003f; //absolute position, set-point updated, change immediately
    ecmSleep(1000);
    *pCw = 0x000f; 
    printf("Target position set to absolute %d.\n", *pTp);
    ecmSleep(10000);
   
    while (1)
    {
        if ((*pSw) & 0x0400) 
        {
            ecmSleep(3000);
            printf("Target reached, position actual value is %d.\n", *pPav);
            *pCw = 0; 
            ecmSleep(1000);
            break;
        }
    }
       
    
    printf("Detaching master...\n");
    result = ecmDetachMaster(hndMaster);
    if (result != ECM_SUCCESS) {
        printf("Failed with %s\n", ecmResultToString(result));
    }

    printf("Deleting master...\n");
    result = ecmDeleteMaster(hndMaster);
    if (result != ECM_SUCCESS) {
        printf("Failed with %s\n", ecmResultToString(result));
    }

    printf("Deleting device...\n");
    result = ecmDeleteDevice(hndDevice);
    if (result != ECM_SUCCESS) {
        printf("Failed with %s\n", ecmResultToString(result));
    }

    ecmInitLibrary(NULL);

    return 0;
}