#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 "freqresp.h"

pthread_t mytask;

volatile float * shm;
volatile struct my_msg_struct * msg;

void *thread_code(void *arg) /* リアルタイムタスク本体 */
{
	hrtime_t s_time;
	float t, t0 = 0.;
	int count_cancel, count_record, count_ref, flag = 0;
	
	double w;
	double y[2], u;
	int i;
	static float refs[PERIOD_MAX];
	
	while(1){
		int ret;
		int err;

		ret = pthread_wait_np();

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

			count_cancel = 0;
			count_record = 0;
			count_ref = 0;
			
			w = 2.0 * M_PI / (double)msg->resolution;
			for(i = 0; i < msg->resolution; i++){
				refs[i] = msg->amp * sin(w * (double)i);
			}
			
			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;
			
			u = refs[count_ref];

			if(u > 3.) u = 3.;
			if(u < -3.) u = -3.;
			da_conv(V_OFFSET + u, msg->spk_num);
			
			ad_conv(&y);

			if(count_cancel < msg->canceling_samples){
				count_cancel++;
			}else{
				if(count_record < BUF_LEN * msg->recording_samples){
					shm[count_record++] = t;
					shm[count_record++] = u;
					shm[count_record++] = y[0];
					shm[count_record++] = y[1];
					/*
					shm[count_record++] = t0;
					t0 = t;
					*/
				}else{
					flag = 0;
					pthread_make_periodic_np(pthread_self(), gethrtime(), 0.1 * 1e9);
				}
			}
			
			if(count_ref < msg->resolution - 1){
				count_ref++;
			}else{
				count_ref = 0;
			}
			
		}
		
	}
	
	return 0; 
}

int
init_module(void)
{
	pthread_attr_t attr;
	struct sched_param sched_param;
	int ret;
	
	if((shm = (volatile float*) mbuff_alloc("data", BUF_LEN * LENGTH_MAX * sizeof(float))) == NULL){
		rtl_printf("mbuff_alloc failed\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("clmodeling_module: init\n");

	return 0;
}

void
cleanup_module(void)
{
	mbuff_free("data",(void*)shm);
	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("clmodeling_module: cleanup\n");
}