#include <main.h>
#include <mqtt/mqtt_client.h>
#include <can/can_client.h>
#include <io/io.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

/// @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);
                //printf("%.3f: 100ms-Cycle %ld...\n", clock_gettime_s() - pinfo->fStartTime, pinfo->cyclecounter);
        }

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

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

        if (nCalled > 0)
        {
                //printf("%.3f: Called 0x%.4X at cycle %ld...\n", clock_gettime_s() - pinfo->fStartTime, nCalled, pinfo->cyclecounter);
        }
}
 

/// @brief Our one and only realtime task
/// @param data 
/// @return 
void *thread_func(void *data)
{
        // Open CAN interface first motor
        if (Can_OpenInterface(0, "can0"))
        {
                printf("Can_OpenInterface() failed!\n");
                return NULL;
        }

        // Connect to mqtt broker
        while (MqttClient_Connect() && (iThreadControl == 0))
        {
                printf("MqttClient_Connect() failed!\n");
                sleep(10);
        }

        // Initialize IO Ports
        if (IO_Init())
        {
                printf("IO_Init() failed!\n");
                return NULL;
        }

        // initialize cyclic task
        periodic_task_init(1, &pinfo);
 
        // "Zündung" ein
        WriteOutputPin(GPIO_OUT_PWRON, HIGH);

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

        // "Zündung" aus
        WriteOutputPin(GPIO_OUT_PWRON, LOW);

        // Disconnect from mqtt broker
        MqttClient_Close();

        // Close CAN interface
        Can_CloseInterface(0);
        
        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))
        {
                printf("Received signal %d\n", signo);
                iThreadControl = -1;    // signal realtime thread to exit
        }
}


/// @brief Hauptfunktion Echtzeit-Task erstellen und starten
/// @param argc 
/// @param argv 
/// @return 
int main(int argc, char* argv[])
{
        struct sched_param param;
        pthread_attr_t attr;
        pthread_t thread;
        int ret;

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

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

        /* Initialize pthread attributes (default values) */
        ret = pthread_attr_init(&attr);
        if (ret) 
        {
                printf("init pthread attributes failed\n");
                goto out;
        }

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

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

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

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

out:
        return ret;
}