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

#ifdef FLOAT
volatile float * time_resp;
#else
volatile double * time_resp;
#endif

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

volatile struct my_msg_struct * msg;

void *thread_code(void *arg)
{
	hrtime_t s_time;
	float t, t0;
	int count_record, flag = 0;

	int i, j, N, count_interval;
	int a[N], b[N];
	double y, w, z, up;
	int AiData[3];

#ifdef FLOAT
	volatile float *p;
	float u;
	float y_pic; // y_pic : y の下 3 ビット分をマスクしたもの
	float x[ORDER_MAX]; // 補償器の状態
	float x_tmp[ORDER_MAX];
#else
	volatile double *p;
	double u;
	double y_pic;
	double x[ORDER_MAX]; // 補償器の状態
	double x_tmp[ORDER_MAX];
#endif

	while(1){
		int ret;
		int err;

		ret = pthread_wait_np();

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

			N = msg->order;
			count_record = 0;
			count_interval = 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, SAMPLING_PERIOD * 1.0e9);
		}

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

			w = AMP * (2. * rand() / (RAND_MAX + 1.) - 1.); // 白色雑音用データを乱数で発生
			da_conv(V_OFFSET + w, 1); // 一次音源スピーカに出力

			ad_conv_raw(&AiData);
			z = (double)(AiData[0] & 0x0fff) * 10. / 4096. - 5. - ERRMIC_OFFSET;
			y = (double)(AiData[1] & 0x0fff) * 10. / 4096. - 5. - REFMIC_OFFSET;
			up = (double)(AiData[2] & 0x0fff) * 10. / 4096. - 5. - REFMIC_OFFSET;

#ifdef FLOAT
			//y_pic = (float)(AiData[1] & 0x0ff8) * 10. / 4096. - 5. - REFMIC_OFFSET; // PIC の ADC 分解能を 8bit とする場合
			y_pic = (float)(AiData[1] & 0x0ffe) * 10. / 4096. - 5. - REFMIC_OFFSET; // PIC の ADC 分解能を 10bit とする場合
#else
			y_pic = (double)(AiData[1] & 0x0ff8) * 10. / 4096. - 5. - REFMIC_OFFSET;
#endif

			if(count_record > BUF_LEN * WAITING_SAMPLES){

				count_interval++;
				if(count_interval == msg->interval){
					count_interval = 0;

					//					da_conv(V_OFFSET + 4.9, 2); // PIC スタート
					da_conv(V_OFFSET, 2); // PIC スタート
					
#ifdef IIR
					for(i = 0; i < N; i++){
						y_pic += a[i+1] * x[i];
					}

					u = b[0] * y_pic;
					for(i = 0; i < N; i++){
						u += b[i+1] * x[i];
					}
#else // IIR
					/* 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++;
					}

#ifdef PIC				
					u += *p * y_pic;
#else
					u += *p * y;
#endif
					
#endif // IIR

					if(u > U_MAX) u = U_MAX;
					if(u < -U_MAX) u = -U_MAX;
					
#ifndef NO_CONTROL
#ifdef PIC
					da_conv_pic(V_OFFSET + u, 0); // 2 次スピーカに出力
#else
					da_conv(V_OFFSET + u, 0); // 2 次スピーカに出力
#endif
#endif
					
#ifdef IIR
					for(i = N - 1; i > 0; i--){
						x[i] = x[i-1];
					}
					x[0] = y_pic;
					
					
#else // IIR				
					/* 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++;
						}
#ifdef PIC					
						x_tmp[i] += *p * y_pic;
#else
						x_tmp[i] += *p * y;
#endif
						p++;
					}
					memcpy(x, x_tmp, sizeof(x));
#endif // IIR
					
				}

			}
				
			if(count_record < BUF_LEN * RECORDING_SAMPLES){
				time_resp[count_record++] = t;
				time_resp[count_record++] = z; // エラーマイク
				time_resp[count_record++] = y; // リファレンスマイク
				time_resp[count_record++] = u; // 制御入力
				time_resp[count_record++] = w; // 外乱入力
				time_resp[count_record++] = up; // PIC が生成した制御入力
				for(i = 0; i < msg->order; i++){
					time_resp[count_record++] = x[i];
				}
				
			}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;

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

#ifdef IIR
	if((a = (volatile float *) mbuff_alloc("a", (ORDER_MAX + 1) * sizeof(float))) == NULL){
		rtl_printf("mbuff_alloc failed for a\n");
		return -1;
	}
	if((b = (volatile float *) mbuff_alloc("b", (ORDER_MAX + 1) * sizeof(float))) == NULL){
		rtl_printf("mbuff_alloc failed for b\n");
		return -1;
	}
#else // IIR
#ifdef FLOAT	
	if((cont_real = (volatile float *) mbuff_alloc("cont_real", (ORDER_MAX + 1) * (ORDER_MAX + 1) * sizeof(float))) == NULL){
		rtl_printf("mbuff_alloc failed for cont_real\n");
		return -1;
	}
#else // FLOAT
	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;
	}
#endif // FLOAT
#endif // IIR

	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);
	//	da_conv(V_OFFSET, 2); // PIC ストップ
	da_conv(V_OFFSET + 4.9, 2); // PIC ストップ

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

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

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

	cleanup_adc();
	cleanup_dac();
	da_conv(V_OFFSET, 0);
	da_conv(V_OFFSET, 1);
	//	da_conv(V_OFFSET, 2); // PIC ストップ
	da_conv(V_OFFSET + 4.9, 2); // PIC ストップ

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