#include "main.h"
#include <pthread.h>
#include <semaphore.h>
#include <unistd.h>
#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <mqueue.h>
#include <fcntl.h>

#define SIZE 8

void check_errno(int error)
{
    if (error < 0)
    {
        perror("Error");
        while (1);
    }
}

void check(int error)
{
    if (error != 0)
    {
        printf("Error: %s\n", strerror(error));
        while (1);
    }
}

mqd_t mqFood;
sem_t semPrintf;

void put(char c)
{
    check_errno( mq_send(mqFood, (char *)&c, sizeof(c), 0) );
}

char get(void)
{
    char c = 0;
    check_errno( mq_receive(mqFood, (char *)&c, sizeof(c), NULL) );
    return c;
}

void *producer(void *arg) // function for producer thread
{
    char c = *(char *)arg;
    check_errno( sem_wait(&semPrintf) );
    check_errno( printf("Thread: %p with argument: %c starts\n", pthread_self(), c) );
    check_errno( sem_post(&semPrintf) );
    for (int i = 0; i < 100; ++i)
    {
        put(c);
    }
    check_errno( sem_wait(&semPrintf) );
    check_errno( printf("Thread: %p stops\n", pthread_self()) );
    check_errno( sem_post(&semPrintf) );
    return NULL;
}

void *consumer(void *arg) // function for consumer thread
{
    check_errno( sem_wait(&semPrintf) );
    check_errno( printf("Thread: %p starts\n", pthread_self()) );
    check_errno( sem_post(&semPrintf) );
    for (int i = 0; i < 200; ++i)
    {
        char c = get();
        check_errno( sem_wait(&semPrintf) );
        check_errno( printf("%c", c) );
        check_errno( fflush(stdout) );
        check_errno( sem_post(&semPrintf) );
    }
    check_errno( sem_wait(&semPrintf) );
    check_errno( printf("Thread: %p stops\n", pthread_self()) );
    check_errno( sem_post(&semPrintf) );
    return NULL;
}

int read_prio(char *process_name)
{
    int prio;
    do
    {
        check_errno( printf("Enter priority for process %s [1..15]: ", process_name) );
        check_errno( fflush(stdout) );
    }
    while (scanf("%d", &prio) != 1 || prio < 1 || prio > 15);
    return prio;
}

void *main_thread(void *arg)
{
    int prioc = read_prio("Consumer");
    int priop1 = read_prio("Frikandel Producer");
    int priop2 = read_prio("Kroket Producer");

    check_errno( printf("Output for Consumer priority = %d ", prioc) );
    check_errno( printf("frikandel Producer priority = %d ", priop1) );
    check_errno( printf("Kroket Producer priority = %d\n", priop2) );

    check_errno( sem_init(&semPrintf, 0, 1) ); // allow one thread exclusively to use printf

    // Zie UM2609 6.2.1.2 Add to registry
    // https://www.st.com/resource/en/user_manual/dm00629856-stm32cubeide-user-guide-stmicroelectronics.pdf
    vQueueAddToRegistry((QueueHandle_t) &semPrintf.xSemaphore, "semPrintf");

    struct mq_attr mqAttrs;
    mqAttrs.mq_maxmsg = SIZE;
    mqAttrs.mq_msgsize = sizeof(char);
    mqAttrs.mq_flags = 0;
    check_errno((int)( mqFood = mq_open("/food", O_RDWR | O_CREAT, 0666, &mqAttrs) ));

    pthread_attr_t ptac, ptap1, ptap2;
    check( pthread_attr_init(&ptac) );
    check( pthread_attr_init(&ptap1) );
    check( pthread_attr_init(&ptap2) );

    check( pthread_attr_setstacksize(&ptac, 1024) );
    check( pthread_attr_setstacksize(&ptap1, 1024) );
    check( pthread_attr_setstacksize(&ptap2, 1024) );

    struct sched_param spc, spp1, spp2;
    check( pthread_attr_getschedparam(&ptac, &spc) );
    check( pthread_attr_getschedparam(&ptap1, &spp1) );
    check( pthread_attr_getschedparam(&ptap2, &spp2) );

    spc.sched_priority = prioc;
    spp1.sched_priority = priop1;
    spp2.sched_priority = priop2;

    check( pthread_attr_setschedparam(&ptac, &spc) );
    check( pthread_attr_setschedparam(&ptap1, &spp1) );
    check( pthread_attr_setschedparam(&ptap2, &spp2) );

    pthread_t ptc, ptp1, ptp2;
    char frikandel = 'F', kroket = 'K';
    check( pthread_create(&ptc, &ptac, consumer, NULL) );
    check( pthread_create(&ptp1, &ptap1, producer, &frikandel) );
    check( pthread_create(&ptp2, &ptap2, producer, &kroket) );

    check( pthread_join(ptc, NULL) );
    check( pthread_join(ptp1, NULL) );
    check( pthread_join(ptp2, NULL) );

    check_errno( sem_destroy(&semPrintf) );

    check( mq_close(mqFood) );
    check( mq_unlink("/food") );

    check( pthread_attr_destroy(&ptac) );
    check( pthread_attr_destroy(&ptap1) );
    check( pthread_attr_destroy(&ptap2) );

    return NULL;
}

int main(void)
{
    Board_Init();

    pthread_attr_t pta;
    check( pthread_attr_init(&pta) );
    check( pthread_attr_setdetachstate(&pta, PTHREAD_CREATE_DETACHED) );
    check( pthread_attr_setstacksize(&pta, 1024) );

    struct sched_param sp;
    check( pthread_attr_getschedparam(&pta, &sp) );
    // The main thread must have the highest priority because this thread will start
    // the other threads and we want to study the interaction between those other threads
    sp.sched_priority = 15;
    check( pthread_attr_setschedparam(&pta, &sp) );

    pthread_t pt;
    check( pthread_create(&pt, &pta, main_thread, NULL) );

    printf("\n");
	vTaskStartScheduler();
	/* We should never get here as control is now taken by the scheduler */

    check( pthread_attr_destroy(&pta) );

    return EXIT_SUCCESS;
}