#include <linux/errno.h>
#include <rtl.h>
#include <time.h>

#include <rtl_sched.h>
#include <rtl_fifo.h>

#include <pthread.h>
#include <mbuff.h>
#include <math.h>

#include "adc.h"
#include "hinf.h"

#define RAND_MAX        2147483647 // stdlib.h を include するとエラーになるので，RAND_MAX をここで定義する

pthread_t mytask;

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

void *thread_code(void *arg)
{
	volatile double * p;

	hrtime_t s_time;
	float t, t0;
	int count_record, flag = 0, count_wait, count_dist;

	int i, j;
	double u, yz[2], w;

	double x[ORDER_MAX]; // controller's state
	double x_tmp[ORDER_MAX];

	double xf_z = ERRMIC_OFFSET, xf_y = REFMIC_OFFSET; // states of HPFs for z and y
	double zf, yf; // output of HPFs

	while(1){
		int ret;
		int err;

		ret = pthread_wait_np();

		if (msg->command == START_TASK) {
			msg->command = STOP_TASK;
			flag = 1;

			count_wait = 0;
			count_record = 0;
			count_dist = 0;
			u = 0.;
			w = 0.;
			for(i = 0; i < msg->order; i++){
				x[i] = 0.;
			}

			s_time = gethrtime();
			pthread_make_periodic_np(pthread_self(), s_time, msg->sampling_period * 1.0e9);
		}

		if(flag == 1){
			  
			t = (gethrtime() - s_time)*1.0e-9;

			count_dist++;
			if(count_dist >= DIST_INTERVAL){
				w = AMP * (2. * rand() / (RAND_MAX + 1.) - 1.); // 白色雑音用データを乱数で発生
				count_dist = 0;
			}

			da_conv(V_OFFSET + w, 0); // D/A output to noise source

			ad_conv(&yz); // A/D input

			// HPFs
			yf = CF*xf_y + DF*yz[0];
			xf_y = AF*xf_y + BF*yz[0];
			zf = CF*xf_z + DF*yz[1];
			xf_z = AF*xf_z + BF*yz[1];
			
			count_wait++;
			if(count_wait > WAITING_SAMPLES){

			if(count_record > BUF_LEN * NOCONT_SAMPLES){

				/* u = C x + D y */
				u = 0.;
				p = &cont_real[msg->order * (msg->order + 1)];
				for(i = 0; i < msg->order; i++){
					u += *p * x[i];
					p++;
				}
				u += *p * yf;
				
				if(u > U_MAX) u = U_MAX;
				if(u < -U_MAX) u = -U_MAX;
#ifdef NO_CONTROL
				u = 0;
#endif
				da_conv(V_OFFSET + u, 1); // 2 次スピーカに出力				
				/* x[k+1] = A x[k] + B y */
				p = cont_real;
				for(i = 0; i < msg->order; i++){
					x_tmp[i] = 0.;
					for(j = 0; j < msg->order; j++){
						x_tmp[i] += *p * x[j];
						p++;
					}
					x_tmp[i] += *p * yf;
					p++;
				}
				memcpy(x, x_tmp, sizeof(x));

			}
			
			if(count_record < BUF_LEN * RECORDING_SAMPLES){
				time_resp[count_record++] = t;
				time_resp[count_record++] = zf; // error output
				//				time_resp[count_record++] = yz[1]; // error output
				time_resp[count_record++] = yf; // reference output
				//				time_resp[count_record++] = yz[0]; // reference output
				time_resp[count_record++] = u; // control input
				time_resp[count_record++] = w; // disturbance input
			}else{
				flag = 0;
				pthread_make_periodic_np(pthread_self(), gethrtime(), 0.1 * 1e9);
			}

			}
		}

	}

       	return 0;
}

int
init_module(void)
{
	pthread_attr_t attr;
	struct sched_param sched_param;
	int ret;

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

	if((cont_real = (volatile double *) mbuff_alloc("cont_real", (ORDER_MAX + 1) * (ORDER_MAX + 1) * sizeof(double))) == NULL){
		rtl_printf("mbuff_alloc failed for cont_real\n");
		return -1;
	}
	if((msg = (volatile struct my_msg_struct *) mbuff_alloc("msg", sizeof(struct my_msg_struct))) == NULL){
		rtl_printf("mbuff_alloc failed\n");
		return -1;
	}
	msg->command = STOP_TASK;

	pthread_attr_init (&attr);       /* 属性の初期化 */
	pthread_attr_setfp_np(&attr, 1); /* FPU の使用を許可 */
	sched_param.sched_priority = 1;  /* 優先順位 */
	pthread_attr_setschedparam (&attr, &sched_param);

	init_adc();
	init_dac();
	da_conv(V_OFFSET, 0);
	da_conv(V_OFFSET, 1);

	ret = pthread_create (&mytask,  &attr, thread_code, (void *)0);
	pthread_make_periodic_np(mytask, gethrtime(), 0.1 * 1e9);

	rtl_printf("hinf_module: init\n");

	return 0;
}

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

	pthread_cancel(mytask);
	pthread_join(mytask, NULL);

	cleanup_adc();
	cleanup_dac();
	da_conv(V_OFFSET, 0);
	da_conv(V_OFFSET, 1);

	rtl_printf("hinf_module: cleanup\n");
}