#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <sys/time.h>
#include <sys/types.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <rtl_fifo.h>
#include <rtl_time.h>
#include <mbuff.h>
#include <signal.h>
#include <math.h>

#include "hinf.h"

#define __STR(f)	#f
#define	STR(f)		__STR(f)

volatile float * time_resp;
volatile double * cont_real;
volatile struct my_msg_struct * msg;

static void
emergency_stop(int d)
{
        msg->command = STOP_TASK;
	mbuff_free("time_resp",(void*)time_resp);
	mbuff_free("cont_real",(void*)cont_real);
	mbuff_free("msg",(void*)msg);

	exit(1);
}

int
main(int argc, char *argv[])
{
	int i, j, count;
	FILE *fp, *fp2, *fp3;

	// 補償器の状態空間実現
        fp = fopen("data/controller.dat", "r");
        //fp = fopen("data/controller_discrete_zoh.dat", "r");
	if(fp == NULL){
		fprintf(stderr, "cannot open controller.dat\n");
		exit(1);
	}

	// 補償器の次数
        fp2 = fopen("data/controller_order.dat", "r");
	if(fp == NULL){
		fprintf(stderr, "cannot open controller_order.dat\n");
		exit(1);
	}

	// 補償器の次数
        fp3 = fopen("data/result.dat", "w");
	if(fp == NULL){
		fprintf(stderr, "cannot open result.dat\n");
		exit(1);
	}

	if((time_resp = (volatile float*) mbuff_alloc("time_resp", BUF_LEN * RECORDING_SAMPLES * sizeof(float))) == NULL){
		fprintf(stderr, "mbuff_alloc failed for time_resp\n");
		return -1;
	}

	if((msg = (volatile struct my_msg_struct*) mbuff_alloc("msg", sizeof(struct my_msg_struct))) == NULL){
		fprintf(stderr, "mbuff_alloc failed \n");
		return -1;
	}

	msg->command = STOP_TASK;

	signal(SIGINT, emergency_stop);

	fscanf(fp, "%lf", &msg->sampling_period);
	msg->sampling_period = 0.2e-3; // fixed 2015.12.25
	fprintf(stderr, "sampling time = %f\n", msg->sampling_period);

	fscanf(fp2, "%d", &msg->order);
	fprintf(stderr, "order of the controller = %d\n", msg->order);

	if((cont_real = (volatile double *) mbuff_alloc("cont_real", (msg->order + 1)*(msg->order + 1) * sizeof(double))) == NULL){
		fprintf(stderr, "mbuff_alloc failed for cont_real\n");
		return -1;
	}

	for(i = 0; i < msg->order; i++){
		for(j = 0; j < msg->order; j++) fscanf(fp, "%lf", &cont_real[i * (msg->order + 1) + j]);
	}
	for(i = 0; i < msg->order; i++) fscanf(fp, "%lf", &cont_real[i * (msg->order + 1) + msg->order]);
	for(j = 0; j < msg->order; j++) fscanf(fp, "%lf", &cont_real[msg->order * (msg->order+1) + j]);
	fscanf(fp, "%lf", &cont_real[msg->order * (msg->order+1) + msg->order]);

	time_resp[BUF_LEN * (RECORDING_SAMPLES - 1)] = -1.;

	/* Now start the tasks */
        fprintf(stderr, "Starting real time task ... ");
	msg->command = START_TASK;
	fprintf(stderr, "done.\n");

        fprintf(stderr, "Waiting for record ... ");
	while (time_resp[BUF_LEN * (RECORDING_SAMPLES - 1)] < 0.); /* 正常にデータが書きこまれれば，正になる */
	fprintf(stderr, "done.\n");

	count = 0;
	for(i = 0; i < RECORDING_SAMPLES; i++){
		for(j = 0; j < BUF_LEN; j++){
			fprintf(fp3, "%g ", time_resp[count++]);
		}
		fprintf(fp3, "\n");
	}

	mbuff_free("time_resp",(void*)time_resp);
	mbuff_free("cont_real",(void*)cont_real);
	mbuff_free("msg", (void*)msg);

	fclose(fp);
	fclose(fp2);
	fclose(fp3);

	return 0;
}