/*
 * CANopen main program file for Linux SocketCAN.
 *
 * @file        main
 * @author      Janez Paternoster
 * @copyright   2015 - 2020 Janez Paternoster
 *
 * This file is part of CANopenSocket, a Linux implementation of CANopen
 * stack with master functionality. Project home page is
 * <https://github.com/CANopenNode/CANopenSocket>. CANopenSocket is based
 * on CANopenNode: <https://github.com/CANopenNode/CANopenNode>.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */


#include "CANopen.h"
#include "CO_OD_storage.h"
#include "CO_Linux_threads.h"
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <sched.h>
#include <signal.h>
#include <errno.h>
#include <sys/epoll.h>
#include <net/if.h>
#include <linux/reboot.h>
#include <sys/reboot.h>

#ifdef CO_USE_APPLICATION
/* Call external application functions. */
#include "application.h"
#endif

#if CO_NO_TRACE > 0
#include "CO_time_trace.h"
#endif

#ifdef CO_USE_309
/* Use DS309-3 standard - ASCII command interface to CANopen: NMT + LSS master, SDO client */
#include "CO_command.h"
#endif

#ifdef CO_MULTI_THREAD
/* Use two nonblocking threads: fast rt-thread for CAN reception + PDO + SYNC
 * processing and slow mainline for other processing. */
#include <pthread.h>
#endif


#define NSEC_PER_SEC            (1000000000)    /* The number of nanoseconds per second. */
#define NSEC_PER_MSEC           (1000000)       /* The number of nanoseconds per millisecond. */
#define TMR_THREAD_INTERVAL_NS  (1000000)       /* Interval of threadTmr in nanoseconds */
#define TMR_THREAD_OVERFLOW_US  (5000)          /* Overflow detect limit for threadTmr in microseconds */
#define INCREMENT_1MS(var)      (var++)         /* Increment 1ms variable in threadTmr */


/* Global variable increments each millisecond. */
volatile uint16_t           CO_timer1ms = 0U;

#ifdef CO_MULTI_THREAD
/* Mutex is locked, when CAN is not valid (configuration state). May be used
 *  from other threads. RT threads may use CO->CANmodule[0]->CANnormal instead. */
pthread_mutex_t             CO_CAN_VALID_mtx = PTHREAD_MUTEX_INITIALIZER;
#endif

/* Other variables and objects */
static int                  rtPriority = -1;    /* Real time priority, configurable by arguments. (-1=RT disabled) */
static int                  mainline_epoll_fd;  /* epoll file descriptor for mainline */
static CO_OD_storage_t      odStor;             /* Object Dictionary storage object for CO_OD_ROM */
static CO_OD_storage_t      odStorAuto;         /* Object Dictionary storage object for CO_OD_EEPROM */
static char                *odStorFile_rom    = "od_storage";       /* Name of the file */
static char                *odStorFile_eeprom = "od_storage_auto";  /* Name of the file */
#ifdef CO_USE_309
static in_port_t            CO_command_socket_tcp_port = 60000; /* default port when used in tcp gateway mode */
#endif
#if CO_NO_TRACE > 0
static CO_time_t            CO_time;            /* Object for current time */
#endif

/* Realtime thread */
#ifdef CO_MULTI_THREAD
static void*                rt_thread(void* arg);
static pthread_t            rt_thread_id;
static int                  rt_thread_epoll_fd;
#endif


/* Signal handler */
volatile sig_atomic_t CO_endProgram = 0;
static void sigHandler(int sig) {
    CO_endProgram = 1;
}


/* Helper functions ***********************************************************/
void CO_errExit(char* msg) {
    perror(msg);
    exit(EXIT_FAILURE);
}

/* send CANopen generic emergency message */
void CO_error(const uint32_t info) {
    CO_errorReport(CO->em, CO_EM_GENERIC_SOFTWARE_ERROR, CO_EMC_SOFTWARE_INTERNAL, info);
    fprintf(stderr, "canopend generic error: 0x%X\n", info);
}


static void printUsage(char *progName) {
fprintf(stderr,
"Usage: %s <CAN device name> [options]\n", progName);
fprintf(stderr,
"\n"
"Options:\n"
"  -i <Node ID>        CANopen Node-id (1..127). If not specified, value from\n"
"                      Object dictionary (0x2101) is used.\n"
"  -p <RT priority>    Realtime priority of RT thread (RT disabled by default).\n"
"  -r                  Enable reboot on CANopen NMT reset_node command. \n"
"  -s <ODstorage file> Set Filename for OD storage ('od_storage' is default).\n"
"  -a <ODstorageAuto>  Set Filename for automatic storage variables from\n"
"                      Object dictionary. ('od_storage_auto' is default).\n");
#ifdef CO_USE_309
fprintf(stderr,
"  -c <Socket path>    Enable command interface for master functionality. \n"
"                      If socket path is specified as empty string \"\",\n"
"                      default '%s' will be used.\n"
"                      Note that location of socket path may affect security.\n"
"                      See 'canopencomm/canopencomm --help' for more info.\n"
, CO_command_socketPath);
fprintf(stderr,
"  -t <port>           Enable command interface for master functionality over tcp, \n"
"                      listen to <port>.\n"
"                      Note that using this mode may affect security.\n"
);
#endif
fprintf(stderr,
"\n"
"See also: https://github.com/CANopenNode/CANopenSocket\n"
"\n");
}


/******************************************************************************/
/** Mainline and RT thread                                                   **/
/******************************************************************************/
int main (int argc, char *argv[]) {
    CO_NMT_reset_cmd_t reset = CO_RESET_NOT;
    CO_ReturnError_t err;
    CO_ReturnError_t odStorStatus_rom, odStorStatus_eeprom;
    intptr_t CANdevice0Index = 0;
    int opt;
    bool_t firstRun = true;

    char* CANdevice = NULL;         /* CAN device, configurable by arguments. */
    bool_t nodeIdFromArgs = false;  /* True, if program arguments are used for CANopen Node Id */
    int nodeId = -1;                /* Use value from Object Dictionary or set to 1..127 by arguments */
    bool_t rebootEnable = false;    /* Configurable by arguments */
#ifdef CO_USE_309
    typedef enum CMD_MODE {CMD_NONE, CMD_LOCAL, CMD_REMOTE} cmdMode_t;
    cmdMode_t commandEnable = CMD_NONE;   /* Configurable by arguments */
#endif

    if(argc < 2 || strcmp(argv[1], "--help") == 0){
        printUsage(argv[0]);
        exit(EXIT_SUCCESS);
    }


    /* Get program options */
    while((opt = getopt(argc, argv, "i:p:rc:t:s:a:")) != -1) {
        switch (opt) {
            case 'i':
                nodeId = strtol(optarg, NULL, 0);
                nodeIdFromArgs = true;
                break;
            case 'p': rtPriority = strtol(optarg, NULL, 0); break;
            case 'r': rebootEnable = true;                  break;
#ifdef CO_USE_309
            case 'c':
                /* In case of empty string keep default name, just enable interface. */
                if(strlen(optarg) != 0) {
                    CO_command_socketPath = optarg;
                }
                commandEnable = CMD_LOCAL;
                break;
            case 't':
                /* In case of empty string keep default port, just enable interface. */
                if(strlen(optarg) != 0) {
                  //CO_command_socket_tcp_port = optarg;
                  int scanResult = sscanf(optarg, "%hu", &CO_command_socket_tcp_port);
                  if(scanResult != 1){ //expect one argument to be extracted
                    printf("ERROR: -t argument \'%s\' is not a valid tcp port\n", optarg);
                    exit(EXIT_FAILURE);
                  }
                }
                commandEnable = CMD_REMOTE;
                break;
#endif
            case 's': odStorFile_rom = optarg;              break;
            case 'a': odStorFile_eeprom = optarg;           break;
            default:
                printUsage(argv[0]);
                exit(EXIT_FAILURE);
        }
    }

    if(optind < argc) {
        CANdevice = argv[optind];
        CANdevice0Index = if_nametoindex(CANdevice);
    }

    if(nodeIdFromArgs && (nodeId < 1 || nodeId > 127)) {
        fprintf(stderr, "Wrong node ID (%d)\n", nodeId);
        printUsage(argv[0]);
        exit(EXIT_FAILURE);
    }

    if(rtPriority != -1 && (rtPriority < sched_get_priority_min(SCHED_FIFO)
                         || rtPriority > sched_get_priority_max(SCHED_FIFO))) {
        fprintf(stderr, "Wrong RT priority (%d)\n", rtPriority);
        printUsage(argv[0]);
        exit(EXIT_FAILURE);
    }

    if(CANdevice0Index == 0) {
        char s[120];
        snprintf(s, 120, "Can't find CAN device \"%s\"", CANdevice);
        CO_errExit(s);
    }


    printf("%s - starting CANopen device with Node ID %d(0x%02X)", argv[0], nodeId, nodeId);


    /* Allocate memory for CANopen objects */
    err = CO_new();
    if (err != CO_ERROR_NO) {
        fprintf(stderr, "Program init - %s - CO_new() failed.\n", argv[0]);
        exit(EXIT_FAILURE);
    }


    /* Verify, if OD structures have proper alignment of initial values */
    if(CO_OD_RAM.FirstWord != CO_OD_RAM.LastWord) {
        fprintf(stderr, "Program init - %s - Error in CO_OD_RAM.\n", argv[0]);
        exit(EXIT_FAILURE);
    }
    if(CO_OD_EEPROM.FirstWord != CO_OD_EEPROM.LastWord) {
        fprintf(stderr, "Program init - %s - Error in CO_OD_EEPROM.\n", argv[0]);
        exit(EXIT_FAILURE);
    }
    if(CO_OD_ROM.FirstWord != CO_OD_ROM.LastWord) {
        fprintf(stderr, "Program init - %s - Error in CO_OD_ROM.\n", argv[0]);
        exit(EXIT_FAILURE);
    }


    /* initialize Object Dictionary storage */
    odStorStatus_rom = CO_OD_storage_init(&odStor, (uint8_t*) &CO_OD_ROM, sizeof(CO_OD_ROM), odStorFile_rom);
    odStorStatus_eeprom = CO_OD_storage_init(&odStorAuto, (uint8_t*) &CO_OD_EEPROM, sizeof(CO_OD_EEPROM), odStorFile_eeprom);


    /* Catch signals SIGINT and SIGTERM */
    if(signal(SIGINT, sigHandler) == SIG_ERR)
        CO_errExit("Program init - SIGINIT handler creation failed");
    if(signal(SIGTERM, sigHandler) == SIG_ERR)
        CO_errExit("Program init - SIGTERM handler creation failed");

    /* increase variable each startup. Variable is automatically stored in non-volatile memory. */
    printf(", count=%u ...\n", ++OD_powerOnCounter);


    while(reset != CO_RESET_APP && reset != CO_RESET_QUIT && CO_endProgram == 0) {
/* CANopen communication reset - initialize CANopen objects *******************/

        printf("%s - communication reset ...\n", argv[0]);


#ifdef CO_MULTI_THREAD
        /* Wait other threads (command interface). */
        pthread_mutex_lock(&CO_CAN_VALID_mtx);
#endif

        /* Wait rt_thread. */
        if(!firstRun) {
            CO_LOCK_OD();
            CO->CANmodule[0]->CANnormal = false;
            CO_UNLOCK_OD();
        }


        /* Enter CAN configuration. */
        CO_CANsetConfigurationMode((void *)CANdevice0Index);


        /* initialize CANopen */
        if(!nodeIdFromArgs) {
            /* use value from Object dictionary, if not set by program arguments */
            nodeId = OD_CANNodeID;
        }

        err = CO_CANinit((void *)CANdevice0Index, 0 /* bit rate not used */);
        if (err == CO_ERROR_NO) {
            err = CO_CANopenInit(nodeId);
        }

        if(err != CO_ERROR_NO) {
            char s[120];
            snprintf(s, 120, "Communication reset - CANopen initialization failed, err=%d", err);
            CO_errExit(s);
        }


        /* initialize OD objects 1010 and 1011 and verify errors. */
        CO_OD_configure(CO->SDO[0], OD_H1010_STORE_PARAM_FUNC, CO_ODF_1010, (void*)&odStor, 0, 0U);
        CO_OD_configure(CO->SDO[0], OD_H1011_REST_PARAM_FUNC, CO_ODF_1011, (void*)&odStor, 0, 0U);
        if(odStorStatus_rom != CO_ERROR_NO) {
            CO_errorReport(CO->em, CO_EM_NON_VOLATILE_MEMORY, CO_EMC_HARDWARE, (uint32_t)odStorStatus_rom);
        }
        if(odStorStatus_eeprom != CO_ERROR_NO) {
            CO_errorReport(CO->em, CO_EM_NON_VOLATILE_MEMORY, CO_EMC_HARDWARE, (uint32_t)odStorStatus_eeprom + 1000);
        }


        /* Configure callback functions for thread control */
//        CO_EM_initCallback(CO->em, threadMain_cbSignal);
//        CO_SDO_initCallback(CO->SDO[0], threadMain_cbSignal);
//        CO_SDOclient_initCallback(CO->SDOclient, threadMain_cbSignal);


#if CO_NO_TRACE > 0
        /* Initialize time */
        CO_time_init(&CO_time, CO->SDO[0], &OD_time.epochTimeBaseMs, &OD_time.epochTimeOffsetMs, 0x2130);
#endif

        /* First time only initialization. */
        if(firstRun) {
            firstRun = false;

            /* Configure epoll for mainline */
            mainline_epoll_fd = epoll_create(4);
            if(mainline_epoll_fd == -1)
                CO_errExit("Program init - epoll_create mainline failed");

            /* Init mainline */
            threadMain_init(mainline_epoll_fd, &OD_performance[ODA_performance_mainCycleMaxTime]);


#ifdef CO_MULTI_THREAD
            /* Configure epoll for rt_thread */
            rt_thread_epoll_fd = epoll_create(2);
            if(rt_thread_epoll_fd == -1)
                CO_errExit("Program init - epoll_create rt_thread failed");

            /* Init threadRT */
            CANrx_threadTmr_init(rt_thread_epoll_fd, TMR_THREAD_INTERVAL_NS, &OD_performance[ODA_performance_timerCycleMaxTime]);

            OD_performance[ODA_performance_timerCycleTime] = TMR_THREAD_INTERVAL_NS/1000; /* informative */

            /* Create rt_thread */
            if(pthread_create(&rt_thread_id, NULL, rt_thread, NULL) != 0)
                CO_errExit("Program init - rt_thread creation failed");

            /* Set priority for rt_thread */
            if(rtPriority > 0) {
                struct sched_param param;

                param.sched_priority = rtPriority;
                if(pthread_setschedparam(rt_thread_id, SCHED_FIFO, &param) != 0)
                    CO_errExit("Program init - rt_thread set scheduler failed");
            }
#else
            /* Init threadRT */
            CANrx_threadTmr_init(mainline_epoll_fd, TMR_THREAD_INTERVAL_NS, &OD_performance[ODA_performance_timerCycleMaxTime]);

            OD_performance[ODA_performance_timerCycleTime] = TMR_THREAD_INTERVAL_NS/1000; /* informative */

            /* Set priority for mainline */
            if(rtPriority > 0) {
                struct sched_param param;

                param.sched_priority = rtPriority;
                if(sched_setscheduler(0, SCHED_FIFO, &param) != 0)
                    CO_errExit("Program init - mainline set scheduler failed");
            }
#endif

#ifdef CO_USE_309
            /* Initialize socket command interface */
            switch(commandEnable) {
              case CMD_LOCAL:
                if(CO_command_init() != 0) {
                    CO_errExit("Socket command interface initialization failed");
                }
                printf("%s - Command interface on socket '%s' started ...\n", argv[0], CO_command_socketPath);
                break;
              case CMD_REMOTE:
                if(CO_command_init_tcp(CO_command_socket_tcp_port) != 0) {
                    CO_errExit("Socket command interface initialization failed");
                }
                printf("%s - Command interface on tcp port '%hu' started ...\n", argv[0], CO_command_socket_tcp_port);
                break;
              default:
                break;
            }
#endif

#ifdef CO_USE_APPLICATION
            /* Execute optional additional application code */
            app_programStart();
#endif
        }


#ifdef CO_USE_APPLICATION
        /* Execute optional additional application code */
        app_communicationReset();
#endif


        /* start CAN */
        CO_CANsetNormalMode(CO->CANmodule[0]);

#ifdef CO_MULTI_THREAD
        pthread_mutex_unlock(&CO_CAN_VALID_mtx);
#endif


        reset = CO_RESET_NOT;

        printf("%s - running ...\n", argv[0]);


        while(reset == CO_RESET_NOT && CO_endProgram == 0) {
/* loop for normal program execution ******************************************/
            int ready;
            struct epoll_event ev;

            ready = epoll_wait(mainline_epoll_fd, &ev, 1, -1);

            if(ready != 1) {
                if(errno != EINTR) {
                    CO_error(0x11100000L + errno);
                }
            }

#ifndef CO_MULTI_THREAD
            else if(CANrx_threadTmr_process(ev.data.fd)) {
                /* code was processed in the above function. Additional code process below */
                INCREMENT_1MS(CO_timer1ms);
                /* Detect timer large overflow */
                if(OD_performance[ODA_performance_timerCycleMaxTime] > TMR_THREAD_OVERFLOW_US && rtPriority > 0) {
                    CO_errorReport(CO->em, CO_EM_ISR_TIMER_OVERFLOW, CO_EMC_SOFTWARE_INTERNAL, 0x22400000L | OD_performance[ODA_performance_timerCycleMaxTime]);
                }
            }
#endif

            else if(threadMain_process(ev.data.fd, &reset, CO_timer1ms)) {
                uint16_t timer1msDiff;
                static uint16_t tmr1msPrev = 0;

                /* Calculate time difference */
                timer1msDiff = CO_timer1ms - tmr1msPrev;
                tmr1msPrev = CO_timer1ms;

                /* code was processed in the above function. Additional code process below */

#ifdef CO_USE_APPLICATION
                /* Execute optional additional application code */
                app_programAsync(timer1msDiff);
#endif

                CO_OD_storage_autoSave(&odStorAuto, CO_timer1ms, 60000);
            }

            else {
                /* No file descriptor was processed. */
                CO_error(0x11200000L);
            }
        }
    }


/* program exit ***************************************************************/
    /* join threads */
#ifdef CO_USE_309
    switch (commandEnable)
    {
      case CMD_LOCAL:
        if (CO_command_clear() != 0) {
          CO_errExit("Socket command interface removal failed");
        }
        break;
      case CMD_REMOTE:
        //nothing to do yet
        break;
      default:
        break;
    }
#endif

    CO_endProgram = 1;
#ifdef CO_MULTI_THREAD
    if(pthread_join(rt_thread_id, NULL) != 0) {
        CO_errExit("Program end - pthread_join failed");
    }
#endif

#ifdef CO_USE_APPLICATION
    /* Execute optional additional application code */
    app_programEnd();
#endif

    /* Store CO_OD_EEPROM */
    CO_OD_storage_autoSave(&odStorAuto, 0, 0);
    CO_OD_storage_autoSaveClose(&odStorAuto);

    /* delete objects from memory */
    CANrx_threadTmr_close();
    threadMain_close();
    CO_delete((void *)CANdevice0Index);

    printf("%s on %s (nodeId=0x%02X) - finished.\n\n", argv[0], CANdevice, nodeId);

    /* Flush all buffers (and reboot) */
    if(rebootEnable && reset == CO_RESET_APP) {
        sync();
        if(reboot(LINUX_REBOOT_CMD_RESTART) != 0) {
            CO_errExit("Program end - reboot failed");
        }
    }

    exit(EXIT_SUCCESS);
}


#ifdef CO_MULTI_THREAD
/* Realtime thread for CAN receive and threadTmr ******************************/
static void* rt_thread(void* arg) {

    /* Endless loop */
    while(CO_endProgram == 0) {
        int ready;
        struct epoll_event ev;

        ready = epoll_wait(rt_thread_epoll_fd, &ev, 1, -1);

        if(ready != 1) {
            if(errno != EINTR) {
                CO_error(0x12100000L + errno);
            }
        }

        else if(CANrx_threadTmr_process(ev.data.fd)) {
            int i;

            /* code was processed in the above function. Additional code process below */
            INCREMENT_1MS(CO_timer1ms);

#if CO_NO_TRACE > 0
            /* Monitor variables with trace objects */
            CO_time_process(&CO_time);
            for(i=0; i<OD_traceEnable && i<CO_NO_TRACE; i++) {
                CO_trace_process(CO->trace[i], *CO_time.epochTimeOffsetMs);
            }
#endif

#ifdef CO_USE_APPLICATION
            /* Execute optional additional application code */
            app_program1ms();
#endif

            /* Detect timer large overflow */
            if(OD_performance[ODA_performance_timerCycleMaxTime] > TMR_THREAD_OVERFLOW_US && rtPriority > 0 && CO->CANmodule[0]->CANnormal) {
                CO_errorReport(CO->em, CO_EM_ISR_TIMER_OVERFLOW, CO_EMC_SOFTWARE_INTERNAL, 0x22400000L | OD_performance[ODA_performance_timerCycleMaxTime]);
            }
        }

        else {
            /* No file descriptor was processed. */
            CO_error(0x12200000L);
        }
    }

    return NULL;
}
#endif /* CO_MULTI_THREAD */