#include <main.h>
#include <mqtt/mqtt_client.h>
#include <can/can_client.h>
#include <io/io.h>
#include <settings/settings.h>

// Period info of the realtime task
struct period_info pinfo;
int iThreadControl = 0;         // 0: thread is running, <0: thread shall exit, >0 thread has exited
int iLogToConsole = 1;


/// @brief send a log message
/// @param prio 
/// @param format 
/// @param  
void mylog(int prio, const char *format, ...)
{
        if (prio >= settings.iDebugLevel)
        {
                va_list args;

                // 1. Initialize the argument list with the last fixed argument
                va_start(args, format);

                // 2. Transfer to vsyslog (instead of syslog)
                // vsyslog accepts a va_list
                vsyslog(prio, format, args);

                // 3. Optional: Output additionally to the console
                // We have to reinitialize the list because va_list is "consumed."
                if (iLogToConsole)
                {
                        va_end(args);
                        va_start(args, format);
                        vfprintf(stderr, format, args);
                        fprintf(stderr, "\n");
                }

                // 4. Cleanup
                va_end(args);
        }
}


/// @brief Initialize period_info with period_ms for cyclic task
/// @param period_ms 
/// @param pinfo 
static void periodic_task_init(long period_ms, struct period_info *pinfo)
{
        /* for simplicity, hardcoding a 1ms period */
        pinfo->period_ns = period_ms * 1000000;
        pinfo->cyclecounter = 0;

        clock_gettime(CLOCK_MONOTONIC, &(pinfo->next_period));
        pinfo->fStartTime = (float)pinfo->next_period.tv_sec + ((float)pinfo->next_period.tv_nsec / 1000000000.0);
}


/// @brief Wait in cyclic task the rest of time until the next cycle
/// @param pinfo 
static void wait_rest_of_period(struct period_info *pinfo)
{
        pinfo->next_period.tv_nsec += pinfo->period_ns;
 
        while (pinfo->next_period.tv_nsec >= 1000000000) {
                /* timespec nsec overflow */
                pinfo->next_period.tv_sec++;
                pinfo->next_period.tv_nsec -= 1000000000;
        }
 
        /* for simplicity, ignoring possibilities of signal wakes */
        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &pinfo->next_period, NULL);
}


/// @brief Get monotonic time in sec as float
/// @return 
float clock_gettime_s()
{
        struct timespec tm;
        clock_gettime(CLOCK_MONOTONIC, &tm);
        return (float)tm.tv_sec + ((float)tm.tv_nsec / 1000000000.0);
}


/// @brief Cyclic task, is called from thread_func every ms
/// @param pinfo 
static void do_cyclic_1ms(struct period_info *pinfo)
{
        uint16_t nCalled = 0;

        // read each cycle CAN data
        Can_ReadData(0);

        if ((pinfo->cyclecounter % 10) == 0)
        {
                // called every 10ms
                nCalled |= 0x1000;
                IO_DoCyclic();               
        }

        if ((pinfo->cyclecounter % 50) == 0)
        {
                // called every 50ms
                nCalled |= 0x0001;
                Can_TransmitMotorPower(0);
        }

        if (((pinfo->cyclecounter + 10) % 100) == 0)
        {
                // called every 100ms
                nCalled |= 0x0010;
                Can_TransmitMotorGear(0);
        }

        if (((pinfo->cyclecounter + 20) % 100) == 0)
        {
                // called every 100ms
                nCalled |= 0x0100;
                MqttClient_Refresher();
        }

        if (((pinfo->cyclecounter + 30) % 500) == 0)
        {
                // called every 500ms
                MqttClient_Publisher();
        }
}
 

/// @brief Our one and only realtime task
/// @param data 
/// @return 
void *thread_func(void *data)
{
        // Initialize IO Ports
        if (IO_Init())
        {
                mylog(LOG_ERR, "IO_Init() failed!");
                return NULL;
        }

        // Open CAN interface first motor
        if (Can_OpenInterface(0, "can0"))
        {
                mylog(LOG_ERR, "Can_OpenInterface() failed!");
                return NULL;
        }

        // Connect to mqtt broker
        while (MqttClient_Connect() && (iThreadControl == 0))
        {
                mylog(LOG_ERR, "MqttClient_Connect() failed!");
                sleep(10);
        }

        // initialize cyclic task
        periodic_task_init(1, &pinfo);
 
        // Ignition on
        WriteOutputPin(GPIO_OUT_PWRON, HIGH);

        // cyclic call of do_cyclic_1ms()
        while (iThreadControl == 0)
        {
                pinfo.cyclecounter++;
                if (pinfo.cyclecounter > CYCLE_COUNTER_MAX)
                {
                        // Reset cycle counter every 24h
                        pinfo.cyclecounter = 1;
                } 
                do_cyclic_1ms(&pinfo);
                wait_rest_of_period(&pinfo);
        }

        // Ignition off
        WriteOutputPin(GPIO_OUT_PWRON, LOW);

        // Disconnect from mqtt broker
        MqttClient_Close();

        // Close CAN interface
        Can_CloseInterface(0);
        
        // signal thread has finnished
        iThreadControl = 1;
        return NULL;
}


/// @brief catch signals and set flag to terminate for the realtime thread
/// @param signo 
void sig_handler(int signo)
{
        if ((signo == SIGINT) || (signo == SIGTERM))
        {
                mylog(LOG_INFO, "Received signal %d", signo);
                iThreadControl = -1;    // signal realtime thread to exit
        }
}


/// @brief Main function, create and start realtime task
/// @param argc 
/// @param argv 
/// @return 
int main(int argc, char* argv[])
{
        struct sched_param param;
        pthread_attr_t attr;
        pthread_t thread;
        int ret;

        // First we have to get the default values of our settings
        Settings_InitDefaultValues();

        openlog("CanRtDriver", LOG_PID | LOG_CONS, LOG_DAEMON);
        mylog(LOG_INFO, "Service started. PID: %d", getpid());

        // Read the settings file after opening the log
        Settings_ReadConfFile();

        // catch signals
        if (signal(SIGTERM, sig_handler) == SIG_ERR)
        {
                mylog(LOG_ERR, "Can't catch SIGTERM");
                exit(-1);
        }
        if (signal(SIGINT, sig_handler) == SIG_ERR)
        {
                mylog(LOG_ERR, "Can't catch SIGINT");
                exit(-2);
        }

        /* Lock memory */
        if(mlockall(MCL_CURRENT | MCL_FUTURE) == -1) 
        {
                mylog(LOG_ERR, "mlockall failed: %m");
                exit(-3);
        }

        /* Initialize pthread attributes (default values) */
        ret = pthread_attr_init(&attr);
        if (ret) 
        {
                mylog(LOG_ERR, "init pthread attributes failed");
                goto out;
        }

        /* Set a specific stack size  */
        ret = pthread_attr_setstacksize(&attr, PTHREAD_STACK_MIN);
        if (ret) 
        {
            mylog(LOG_ERR, "pthread setstacksize failed");
            goto out;
        }

        /* Set scheduler policy and priority of pthread */
        ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
        if (ret) 
        {
                mylog(LOG_ERR, "pthread setschedpolicy failed");
                goto out;
        }
        param.sched_priority = 99;  // Priority between 1 (low) and 99() high)
        ret = pthread_attr_setschedparam(&attr, &param);
        if (ret) 
        {
                mylog(LOG_ERR, "pthread setschedparam failed");
                goto out;
        }
        /* Use scheduling parameters of attr */
        ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
        if (ret) 
        {
                mylog(LOG_ERR, "pthread setinheritsched failed");
                goto out;
        }

        
        /* Create a pthread with specified attributes */
        iThreadControl = 0;
        ret = pthread_create(&thread, &attr, thread_func, NULL);
        if (ret) 
        {
                mylog(LOG_ERR, "create pthread failed");
                goto out;
        }

        // join the thread and wait for it to exit
        ret = pthread_join(thread, NULL);
        if (ret)
        {
                mylog(LOG_ERR, "faild to join thread!");
        }

out:
        mylog(LOG_INFO, "Service quit.");
        closelog();

        return ret;
}