#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_EXCL|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;
}