#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/slab.h>

#include <rtai.h>
#include <rtai_sched.h>
#include <rtai_shm.h>
#include <rtai_nam2num.h>

#include <rtai_math.h>

MODULE_DESCRIPTION("Frequency Response Experiment");
MODULE_AUTHOR("Y. Kobayashi");
MODULE_LICENSE("GPL");

#define STACK_SIZE    4000

#include "adc.h"
#include "freqresp.h"

static RT_TASK thread;

static float * data;
static struct my_msg_struct * msg;
static RTIME tick_period;

static void fun(long t_dummy) /* リアルタイムタスク本体 */
{
	RTIME 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;

		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);
			}
			
		       	//tick_period = start_rt_timer(nano2count((msg->sampling_period*1e9)));
			//tick_period = start_rt_timer(nano2count((msg->sampling_period)));

			s_time = rt_get_time();
			printk("pass 1: before rt_task_make_periodic_relative_ns\n");
			rt_task_make_periodic_relative_ns(&thread, 10000000, msg->sampling_period*1e9);
			//rt_task_make_periodic_relative_ns(&thread, 200000, 200000);
			printk("pass 2: after rt_task_make_periodic_relative_ns\n");
		}
		
		if(flag == 1){
			
			t = count2nano(rt_get_time() - 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);
			//			y[0]=0;
			//			y[1]=0;

			if(count_cancel < msg->canceling_samples){
				count_cancel++;
			}else{
				if(count_record < BUF_LEN * msg->recording_samples){
					data[count_record++] = t;
					data[count_record++] = u;
					data[count_record++] = y[0];
					data[count_record++] = y[1];
				}else{
					flag = 0;
					printk("pass 3: flag = 0\n");
					//					rt_task_make_periodic(&thread, rt_get_time(), tick_period*PERIOD_COUNT);
					//rt_task_make_periodic(&thread, rt_get_time(), tick_period);
					rt_task_make_periodic_relative_ns(&thread, 10000000, 10000000);
				}
			}
			
			if(count_ref < msg->resolution - 1){
				count_ref++;
			}else{
				count_ref = 0;
			}
			
		}
		
		rt_task_wait_period();
		
	}
	
}

int
init_module(void)
{
	RTIME now;
	
	init_adc();
	init_dac();
	da_conv(V_OFFSET, 0);
	da_conv(V_OFFSET, 1);
	da_conv(V_OFFSET, 2);

	if((data = (float *)rtai_kmalloc(nam2num("data"), BUF_LEN * LENGTH_MAX * sizeof(float))) == NULL){
	  return -ENOMEM;
	}

	if((msg = (struct my_msg_struct *)rtai_kmalloc(nam2num("msg"), sizeof(struct my_msg_struct))) == NULL){
	  return -ENOMEM;
	}
	msg->command = STOP_TASK;

	rt_task_init(&thread, fun, 0, STACK_SIZE, 0, 1, 0); // 後ろから 2 番地目の 1 = use_fpu を有効にしておかないと、data[] の一部に nan が入る
	//	tick_period = start_rt_timer(nano2count(100000000)); // 最初は 0.1 秒周期で動作させる
	//            tick_period = start_rt_timer(nano2count(200000));
        //      tick_period = start_rt_timer(nano2count(10000));
	rt_linux_use_fpu(1);
	rt_set_oneshot_mode();
	start_rt_timer(0);
	rt_task_make_periodic_relative_ns(&thread, 10000000, 10000000);

	return 0;
}

void
cleanup_module(void)
{
  stop_rt_timer();
  rt_busy_sleep(10000000);
  rt_task_delete(&thread);
  rtai_kfree(nam2num("data"));
  rtai_kfree(nam2num("msg"));

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