/********************************************************************************/
/*                   Demo source for esd EtherCAT Master stack                  */
/*                                                                              */
/*              Copyright by Shanghai ESD Electric Technology Co., Ltd.         */
/*------------------------------------------------------------------------------*/
/*          Filename:       ecmC2ESendCANMsg.c                                  */
/*          Date:           2023-06-13                                          */
/*          Language:       ANSI C                                              */
/*          Targetsystem:   Linux                                               */
/*          Purpose:        Demostrate how to send CAN messages with 29 bit CAN */
/*                          ID as well as 11 bit CAN at master side.            */
/*          Author:         Bob Tu                                              */
/*------------------------------------------------------------------------------*/ 
/* Revision history:                                                            */
/*------------------------------------------------------------------------------*/
/* v1.0     Birth of module                                                     */
/********************************************************************************/

#include <ecm.h>
#include <stdio.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 *pusTxCountGateway = NULL;
static uint16_t *pusTxCountApp = NULL;
static uint16_t *pusTxTransNum = NULL;
static uint16_t *pusNumOfTxMsgs = NULL;
static uint8_t *pucTxMsg = NULL;

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;
    }
    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_VAR_DESC VarDesc;
    ECM_PROC_CTRL ProcCtrl;

    uint8_t byteStream[16];
    uint32_t canid = 0;
    uint8_t len = 8;
    uint8_t content[8] = {0x1f,0x2e,0x3d,0x4c,0x5b,0x6a,0x79,0x88};
    uint8_t rtr = 0;
    uint8_t is29bit = 0;
    

    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_only_can-ethercat_29bit_1mbps.xml";
    cfgInitData.ulFlags = ECM_FLAG_CFG_KEEP_PROCVARS | ECM_FLAG_CFG_VIRTUAL_VARS;
    result = ecmReadConfiguration(&cfgInitData, &hndDevice, &hndMaster);
    if (result != ECM_SUCCESS) {
        printf("Reading configuration failed with %s\n", ecmResultToString(result));
        ecmInitLibrary(NULL);
        return(result);
    }

    result = ecmLookupVariable(hndMaster, "CAN TxPDO-Map.TX Counter", &VarDesc, ECM_FLAG_GET_FIRST);
    if (ECM_SUCCESS == result) {
        result = ecmGetDataReference(hndMaster, ECM_INPUT_DATA, VarDesc.ulBitOffs / 8, VarDesc.usBitSize / 8 , (void **)&pusTxCountGateway);
        if (result != ECM_SUCCESS) {
            printf("Failed to get reference to virtual variable Output\n");
        }
    }

    result = ecmLookupVariable(hndMaster, "CAN RxPDO-Map.TX Counter", &VarDesc, ECM_FLAG_GET_FIRST);
    if (ECM_SUCCESS == result) {
        result = ecmGetDataReference(hndMaster, ECM_OUTPUT_DATA, VarDesc.ulBitOffs / 8, VarDesc.usBitSize / 8 , (void **)&pusTxCountApp);
        if (result != ECM_SUCCESS) {
            printf("Failed to get reference to virtual variable Output\n");
        }
    }

    result = ecmLookupVariable(hndMaster, "CAN RxPDO-Map.Number of TX Messages", &VarDesc, ECM_FLAG_GET_FIRST);
    if (ECM_SUCCESS == result) {
        result = ecmGetDataReference(hndMaster, ECM_OUTPUT_DATA, VarDesc.ulBitOffs / 8, VarDesc.usBitSize / 8 , (void **)&pusNumOfTxMsgs);
        if (result != ECM_SUCCESS) {
            printf("Failed to get reference to virtual variable Output\n");
        }
    }

    result = ecmLookupVariable(hndMaster, "CAN TxPDO-Map.TX Transaction Number", &VarDesc, ECM_FLAG_GET_FIRST);
    if (ECM_SUCCESS == result) {
        result = ecmGetDataReference(hndMaster, ECM_INPUT_DATA, VarDesc.ulBitOffs / 8, VarDesc.usBitSize / 8 , (void **)&pusTxTransNum);
        if (result != ECM_SUCCESS) {
            printf("Failed to get reference to virtual variable Output\n");
        }
    }

    result = ecmLookupVariable(hndMaster, "CAN RxPDO-Map.TX Message 1", &VarDesc, ECM_FLAG_GET_FIRST);
    if (ECM_SUCCESS == result) {
        result = ecmGetDataReference(hndMaster, ECM_OUTPUT_DATA, VarDesc.ulBitOffs / 8, VarDesc.usBitSize / 8 , (void **)&pucTxMsg);
        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;
    // ProcCtrl.pfnBeginCycle  = CycleStartHandler;
    // ProcCtrl.pfnEndCycle    = CycleEndHandler;
    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);
    }

    // Send all 11-bit CAN-ID (0-7ff) msg
    while (1)
    {   
        ECM_INIT(byteStream);
        printf("can-id 11-bit (Hex): %03x \n", canid);
        printf("len: %d \n", len);
        printf("rtr: %d \n", rtr);

        byteStream[0] = 0;
        byteStream[1] = 0;
        byteStream[2] |= len;
        byteStream[3] = 0;
        byteStream[4] = (canid & 0xff);
        byteStream[5] = ((canid & 0xff00) >> 8);
        byteStream[6] = ((canid & 0xff0000) >> 16);
        byteStream[7] = ((canid & 0x1f000000) >> 24);
        byteStream[7] |= (rtr<<6);
        byteStream[7] |= (is29bit<<7);

        for (int i=0;i<len;i++)
            byteStream[i+8] = content[i];
        
        for (int k=0;k<16;k++)
            printf("%02x ", byteStream[k]);
        printf("\n\n");

        for (int j=0;j<16;j++){
            *pucTxMsg = byteStream[j];
            pucTxMsg++;
        }

        *pusTxTransNum = 0;
        *pusNumOfTxMsgs = 1;
        *pusTxCountApp = *pusTxCountApp + 1;
        pucTxMsg=pucTxMsg-16;
        ecmSleep(10); 
        
        canid++;
        if (canid==0x800) 
        {   
            canid = 0;
            is29bit=1;
            break;
        }
    }

// Send all 29-bit CAN-ID (0-1fffffff) msg
    while (1)
    {   
        ECM_INIT(byteStream);
        printf("can-id 29-bit (Hex): %08x \n", canid);
        printf("len: %d \n", len);
        printf("rtr: %d \n", rtr);

        byteStream[0] = 0;
        byteStream[1] = 0;
        byteStream[2] |= len;
        byteStream[3] = 0;
        byteStream[4] = (canid & 0xff);
        byteStream[5] = ((canid & 0xff00) >> 8);
        byteStream[6] = ((canid & 0xff0000) >> 16);
        byteStream[7] = ((canid & 0x1f000000) >> 24);
        byteStream[7] |= (rtr<<6);
        byteStream[7] |= (is29bit<<7);

        for (int i=0;i<len;i++)
            byteStream[i+8] = content[i];
        
        for (int k=0;k<16;k++)
            printf("%02x ", byteStream[k]);
        printf("\n\n");

        for (int j=0;j<16;j++){
            *pucTxMsg = byteStream[j];
            pucTxMsg++;
        }

        *pusTxTransNum = 0;
        *pusNumOfTxMsgs = 1;
        *pusTxCountApp = *pusTxCountApp + 1;
        pucTxMsg=pucTxMsg-16;
        ecmSleep(10); 
        
        canid++;
        if (canid==0x20000000) 
        {   
            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;

}