#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <pthread.h>
#include <mqueue.h>

double readTemp(void) {
//..dummy: should read temperature from ADC
    static double val = 0.0;
    val += 0.5;
    if (val > 20.0)
        val = 0.0;
    return val;
}

void writeSwitch(int i) {
//  dummy: should write to switch
}

double readPres(void) {
//  dummy: should read pressure from ADC
    static double val = 20.0;
    val -= 0.4;
    if (val < 0.0)
        val = 20.0;
    return val;
}

void writeDAC(double d) {
//  dummy: should write to DAC
}

int tempControl(double temp) {
//  dummy: should use better algorithm
    static struct timespec ms100 = {0, 100000000};
    nanosleep(&ms100, NULL);
    return temp > 10.0;
}

double presControl(double pres) {
//  dummy: should use better algorithm
    static struct timespec ms100 = {0, 100000000};
    nanosleep(&ms100, NULL);
    return 10.0 - pres;
}

void check(int error) {
    if (error != 0) {
        fprintf(stderr, "Error: %s\n", strerror(error));
        mq_unlink("/mq_par74");
        exit(EXIT_FAILURE);
    }
}

void check_errno(int error) {
    if (error < 0) {
        perror("Error: ");
        mq_unlink("/mq_par74");
        exit(EXIT_FAILURE);
    }
}
    
void* tempThread(void* p) {
    double temp;
    int switch_;
    mqd_t m;
    char buffer[128];
    int i;
    check_errno( m = mq_open("/mq_par74", O_WRONLY) );
    for (i = 0; i < 10; i++) {
        int n = 0;
        temp = readTemp();
        check_errno( n = snprintf(buffer, sizeof buffer, "temperature = %4.1lf", temp) );
        switch_ = tempControl(temp);
        check_errno( snprintf(&buffer[n], sizeof buffer - n, ", switch = %d\n", switch_) );
        check_errno( mq_send(m, buffer, sizeof buffer, 2) );
        writeSwitch(switch_);
        sleep(3);
    }
    check_errno( mq_close(m) );
    return NULL;
}

void* presThread(void* p) {
    double pres, dac;
    mqd_t m;
    char buffer[128];
    int i;
    check_errno( m = mq_open("/mq_par74", O_WRONLY) );
    for (i = 0; i < 30; i++) {
        int n = 0;
        pres=readPres();
        check_errno( n = snprintf(buffer, sizeof buffer, "pressure = %4.1lf", pres) );
        dac=presControl(pres);
        check_errno( snprintf(&buffer[n], sizeof buffer - n, ", DAC = %5.1lf\n", dac) );
        check_errno( mq_send(m, buffer, sizeof buffer, 3) );
        writeDAC(dac);
        sleep(1);
    }
    check_errno( mq_close(m) );
    return NULL;
}

void* displayThread(void* p) {
    mqd_t m;
    struct mq_attr ma;
    char buffer[128];
    int doorgaan = 1;
    check_errno( m = mq_open("/mq_par74", O_RDONLY) );
    while (doorgaan) {
        do {
            check_errno( mq_receive(m, buffer, sizeof buffer, NULL) );
            if (strcmp(buffer, "CLOSE") == 0) 
                doorgaan = 0;
            else
                check_errno( printf(buffer) );
            check_errno( mq_getattr(m, &ma) );
        }
        while (ma.mq_curmsgs > 0); 
        sleep(5);
        check_errno( putchar('\007') );
    }
    check_errno( mq_close(m) );
}

int main(void) {
    pthread_t t1, t2, t3;
    mqd_t m;
    struct mq_attr ma;

    ma.mq_maxmsg = 40;
    ma.mq_msgsize = 128;
    
    check_errno( m = mq_open("/mq_par74", O_CREAT|O_RDWR , 0666, &ma) );

    check( pthread_create(&t1, NULL, tempThread, NULL) );
    check( pthread_create(&t2, NULL, presThread, NULL) );
    check( pthread_create(&t3, NULL, displayThread, NULL) );

    check( pthread_join(t1, NULL) );
    check( pthread_join(t2, NULL) );
    check_errno( mq_send(m, "CLOSE", 6, 1) );
    
    check( pthread_join(t3, NULL) );

    check_errno( mq_close(m) );
    check_errno( mq_unlink("/mq_par74") );

    return EXIT_SUCCESS;
}