#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)

// cont_real ... 補償器の状態空間実現(A,B,C,D 行列の係数) or IIR フィルタの係数
#ifdef IIR
volatile float * a;
volatile float * b;
#else // IIR
#ifdef FLOAT
volatile float * time_resp;
volatile float * cont_real;
#else // FLOAT
volatile double * time_resp;
volatile double * cont_real;
#endif // FLOAT
#endif // IIR

volatile struct my_msg_struct * msg;

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

#ifdef IIR
	mbuff_free("a",(void*)a);
	mbuff_free("b",(void*)b);
#else
	mbuff_free("cont_real",(void*)cont_real);
#endif

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

	exit(1);
}

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

	// 補償器の状態空間実現
#ifdef IIR	
        //fp = fopen("data/controller_IIR_r.dat", "r");
#else
	//fp = fopen("data/controller_discrete_zoh.dat", "r"); // strans の出力をブロック毎に正準形に変換
	//fp = fopen("data/controller_r.dat", "r"); // 一般形(構造の制約なし)
        //fp = fopen("data/controller0_r.dat", "r"); // 可制御正準形
	//fp = fopen("data/controller_strans_r.dat", "r"); // strans の出力
	fp = fopen("data/controller0_mode_r.dat", "r"); // strans の出力をブロック毎に正準形に変換
	//fp = fopen("data/controller_r_d.dat", "r");
        //fp = fopen("data/controller0_r_d.dat", "r");
	//fp = fopen("data/controller_strans_r_d.dat", "r");
	//fp = fopen("data/controller0_mode_r_d.dat", "r");
	//fp = fopen("data/controller.dat", "r");
#endif
	if(fp == NULL){
		fprintf(stderr, "cannot open controller.dat\n");
		exit(1);
	}

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

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

#ifdef FLOAT
	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;
	}
#else
	if((time_resp = (volatile double*) mbuff_alloc("time_resp", BUF_LEN * RECORDING_SAMPLES * sizeof(double))) == NULL){
		fprintf(stderr, "mbuff_alloc failed for time_resp\n");
		return -1;
	}
#endif

	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, "%f", &sampling_period);
	msg->interval = sampling_period / SAMPLING_PERIOD;
	fprintf(stderr, "interval = %d\n", msg->interval);

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

#ifdef IIR
	if((a = (volatile float *) mbuff_alloc("a", (ORDER_MAX + 1) * sizeof(float))) == NULL){
		fprintf(stderr, "mbuff_alloc failed for a\n");
		return -1;
	}
	if((b = (volatile float *) mbuff_alloc("b", (ORDER_MAX + 1) * sizeof(float))) == NULL){
		fprintf(stderr, "mbuff_alloc failed for b\n");
		return -1;
	}
	for(i = 0; i <= msg->order; i++){
		fscanf(fp, "%f", &a[i]);
	}
	for(i = 0; i <= msg->order; i++){
		fscanf(fp, "%f", &b[i]);
	}
	
#else // IIR
#ifdef FLOAT
	if((cont_real = (volatile float *) mbuff_alloc("cont_real", (msg->order + 1)*(msg->order + 1) * sizeof(float))) == 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, "%f", &cont_real[i * (msg->order + 1) + j]);
	}
	for(i = 0; i < msg->order; i++) fscanf(fp, "%f", &cont_real[i * (msg->order + 1) + msg->order]);
	for(j = 0; j < msg->order; j++) fscanf(fp, "%f", &cont_real[msg->order * (msg->order+1) + j]);
	fscanf(fp, "%f", &cont_real[msg->order * (msg->order+1) + msg->order]);

#else // FLOAT
	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]);
#endif // FLOAT

#endif // IIR

	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, "%.10e ", time_resp[count++]);
		}
		fprintf(fp3, "\n");
	}

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

#ifdef IIR
	mbuff_free("a",(void*)a);
	mbuff_free("b",(void*)b);
#else
	mbuff_free("cont_real",(void*)cont_real);
#endif

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

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

	return 0;
}