#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 <stdio.h>
#include <sys/io.h>
#include <unistd.h>
#include <stdlib.h>
 
#include "adc.h"
#include "adda.h"
#include "freqresp.h"
 
//#define DIOBASEADD 0xdf80 //
 
 
/////////////////
//#define V_R 200
//#define Kp 0.063      //比例係数
//#define Kp 0.0315
//#define Ki 0.004165   //積�?係数
//#define Ki 0.0833
/////////////////
 
 
static pthread_t output_task_1;
struct sched_param p;
struct timespec treq, trem;
 
//int step_refs[PERIOD_MAX];
//int dio_refs[PERIOD_MAX];

volatile float * shm;
volatile struct my_msg_struct * msg;

float torq_volt_conv(float torq){//_$B%H%k%/A`:nNL$rEE05$KJQ49_(B

    float volt_max[2];
    volt_max[0] = (float)Pn400_0;
    volt_max[1] = (float)Pn400_1;
    float volt; 

    volt =  torq / (float)RATED_TORQ * (volt_max[msg->motor_num]);
    
    if(volt > (volt_max[msg->motor_num] *0.33)) volt = (volt_max[msg->motor_num] *0.33);
    if(volt < -(volt_max[msg->motor_num] *0.33)) volt = -(volt_max[msg->motor_num] *0.33);
 
    return volt;
}
float torq_volt_convD(float torq){

    float volt_max[2];
    volt_max[0] = (float)Pn400_0;
    volt_max[1] = (float)Pn400_1;
    float volt; 

    volt =  torq / (float)RATED_TORQ * (volt_max[msg->motorD_num]);
    
    if(volt > (volt_max[msg->motorD_num] *0.33)) volt = (volt_max[msg->motorD_num] *0.33);
    if(volt < -(volt_max[msg->motorD_num] *0.33)) volt = -(volt_max[msg->motorD_num] *0.33);
 
    return volt;
}
 
float ang_conv(long counter_data){
    return (float)counter_data / (360 / (float)Pn212 * 1);//1_$BD~G\_(B
}
 
static double
read_theta(int ch)
{
        int d;
 
        cnt_latch();
        cnt_read(&d, 0, ch);
        //return 2.* M_PI/1024. * d;
    return d;
}
 
/************************************************/
/* realtime task                                */
/************************************************/
static void* outputfunc1(void *t)
{
    hrtime_t s_time;
    float t; 
    //float t0 = 0.; 
    int count_cancel, count_record, count_ref, flag = 0;
    
    double w;
    //double y, u, torque_ref, speed_ref, volt_ref;
    //double theta_rad, theta_rad_before=0.0, dtheta_rad;
    int i;
    static float refs[PERIOD_MAX];
 
////////////////////////////////
    double torq_r;
    double volt_torq_r;
    double volt_torq_rD;
    //double tmp;
    double thetaM_rad[2], thetaM_rad_before[2], thetaM_rad_before2[2], speedM_rad[2], Kp[2], Ki[2], ya1, yb1, ya2, yb2, ua, ub;
    int cnt[2], cnt_before[2];
    //double thetaM_rad[2], thetaM_rad_before[2], speedM_rad[2], ya1, yb1, ya2, yb2, ua, ub;
    //double theta_rad_before[0]=0.0;
    //Kp[0] = 0.00315;
    Kp[0] = 0.02;
    //Kp[0] = 0.063;
    Kp[1] = 0.02;
    //Kp[1] = 0.063;

    Ki[0] = 0.01;
    //Ki[0] = 0.004165;
    Ki[1] = 0.01;
    //Ki[1] = 0.004165;

    double V_R_rad;
    
    double v_e;//P
    double v_i,pv_i;//PI
 
    double Ts, ya3, yb3; // added (2017.6.22)

    pv_i = 0;
////////////////////////////////
    
    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;
            pv_i = 0;
            ya1 = 0; // 1st sin component for speed output driving motor
            yb1 = 0; // 1st cos component for speed output driven motor
            ya2 = 0; // 1st sin component for speed output driving motor
            yb2 = 0; // 1st cos component for speed output driven motor
            ua = 0; // 1st sin component for torque input
            ub = 0; // 1st cos component for torque input
            cnt_init_module();
	    ya3 = 0; // added (2017.6.22)
	    yb3 = 0; // added (2017.6.22)

            w = 2.0 * M_PI / (double)msg->resolution;
            for(i = 0; i < msg->resolution; i++){
                refs[i] = msg->amp * sin(w * (double)i) + msg->offset;
            }
            
            s_time = gethrtime();
            pthread_make_periodic_np(pthread_self(), s_time, msg->sampling_period * 1.0e9);
            //          thetaM_rad_before = (double)read_theta(1) / (double)Pn212 * 2 * M_PI;//角度割�?rad
            thetaM_rad_before[0] = (double)read_theta(0) / (double)Pn212 * 2 * M_PI;//driving motor rad
            thetaM_rad_before[1] = (double)read_theta(1) / (double)Pn212 * 2 * M_PI;//driven motor rad
            thetaM_rad_before2[0] = thetaM_rad_before[0];//driving motor 
            thetaM_rad_before2[1] = thetaM_rad_before[1];//driven motor
	    cnt_before[0] = read_theta(0);
	    cnt_before[1] = read_theta(1);
        }
        
        if(flag == 1){

            t = (gethrtime() - s_time)*1.0e-9;
 
            //volt_ref = refs[count_ref];//0.4 * refs[count_ref];//0.3_$B$O;W9M:x8m$NCM_(B
            //daconv(0,bit16_conv(volt_ref));
 
/////////////////////////////////////////
 
              //thetaM_rad = (double)read_theta(1) / (double)Pn212 * 2 * M_PI;//角度割�?rad
              //speedM_rad = (thetaM_rad - thetaM_rad_before) / msg->sampling_period;//回転速度rad/sec
              //thetaM_rad_before = thetaM_rad;//差�?

	      // fixed 2018.6.21 (due to limitation of the counter board, Contec CNT24-4: maximum count is 0xffffff)
	      cnt[0] = read_theta(0);
	      if(cnt_before[0] - cnt[0] > 0x7fffff){
		cnt[0] = cnt[0] + 0xffffff;
	      }
	      cnt[1] = read_theta(1);
	      if(cnt_before[1] - cnt[1] > 0x7fffff){
		cnt[1] = cnt[1] + 0xffffff;
	      }

	      thetaM_rad[0] = (double)cnt[0] / (double)Pn212 * 2 * M_PI;//rad driving motor
	      thetaM_rad[1] = (double)cnt[1] / (double)Pn212 * 2 * M_PI;//rad driven motor

	      //              thetaM_rad[0] = (double)read_theta(0) / (double)Pn212 * 2 * M_PI;//rad driving motor
	      //              thetaM_rad[1] = (double)read_theta(1) / (double)Pn212 * 2 * M_PI;//rad driven motor
            
	      //	      ad_conv(&Ts); // added (2017.6.22)

              speedM_rad[0] = (thetaM_rad[0] - thetaM_rad_before2[0]) / (2 * msg->sampling_period);//rad/sec driving motor
              speedM_rad[1] = (thetaM_rad[1] - thetaM_rad_before2[1]) / (2 * msg->sampling_period);//rad/sec driven motor
              
              thetaM_rad_before2[0] = thetaM_rad_before[0];//driving motor 
              thetaM_rad_before2[1] = thetaM_rad_before[1];//driven motor

              thetaM_rad_before[0] = thetaM_rad[0];//driving motor 
              thetaM_rad_before[1] = thetaM_rad[1];//driven motor

              cnt_before[0] = cnt[0];//driving motor 
              cnt_before[1] = cnt[1];//driven motor
            
              //PI制御
              //V_R_rad = (2 * M_PI * V_R)/60;//rpmからrad/secへ 
              V_R_rad = refs[count_ref];
              v_e = V_R_rad - speedM_rad[msg->motor_num]; //速度偏差 = �?��速度 - 現在の速度
              v_i = pv_i + v_e * msg->sampling_period;      
              //torq_r = Kp * v_e + Ki * v_i ;//ここでPI制御
	      torq_r = Kp[msg->motor_num] * v_e + Ki[msg->motor_num] * v_i + (double)RATED_TORQ * (double)twist;
              pv_i = v_i;
            
              //操作量を電圧に変換
              volt_torq_r = torq_volt_conv(torq_r);
	      volt_torq_rD = torq_volt_convD(-((double)RATED_TORQ * (double)twist));
              da_conv(volt_torq_r, msg->motor_num);
              da_conv(volt_torq_rD, msg->motorD_num);
 
////////////////////////////////////////
              
              //            theta_rad = (double)read_theta(1) / (double)Pn212 * 2 * M_PI;//角度割�?
              //            dtheta_rad = (theta_rad - theta_rad_before) / msg->sampling_period;//回転速度rpm
              //            theta_rad_before = theta_rad;
            
              if(count_cancel < msg->canceling_samples){
                count_cancel++;
              }else{
                if(count_record < BUF_LEN * msg->recording_samples){
                  //tmp = speedM_rad*60./(2.0*M_PI);//[rpm]
                  ya1 += speedM_rad[0]*sin(msg->omega*t); // driving motor sin 
                  yb1 += speedM_rad[0]*cos(msg->omega*t); // driving motor cos 
                  ya2 += speedM_rad[1]*sin(msg->omega*t); // driven motor sin
                  yb2 += speedM_rad[1]*cos(msg->omega*t); // driven motor cos
 
                  ua += torq_r*sin(msg->omega*t);
                  ub += torq_r*cos(msg->omega*t);

                  shm[count_record++] = t;
                  shm[count_record++] = torq_r;
		  shm[count_record++] = speedM_rad[0];
                  shm[count_record++] = torq_r;
		  shm[count_record++] = speedM_rad[1];
		  //		  shm[count_record++] = cnt[0];
                }else{
                  msg->ya1 = 2.* ya1 / msg->recording_samples; // driving motor average
                  msg->yb1 = 2.* yb1 / msg->recording_samples; // driving motor average
                  msg->ya2 = 2.* ya2 / msg->recording_samples; // driven motor average
                  msg->yb2 = 2.* yb2 / msg->recording_samples; // driven motor average
 
                  msg->ua = 2.* ua / msg->recording_samples; // average
                  msg->ub = 2.* ub / msg->recording_samples; // average

                  da_conv(0.0, msg->motor_num);
                  da_conv(0.0, msg->motorD_num);
                  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; 
}
 
 
 
/************************************************/
/* initialize module                            */
/************************************************/
int init_module(void)
{
 
 
    if((shm = (volatile float*) mbuff_alloc("data", BUF_LEN * LENGTH_MAX * sizeof(float))) == NULL){
        rtl_printf("mbuff_alloc failed 1\n");
        return -1;
    }
 
    if((msg = (volatile struct my_msg_struct *) mbuff_alloc("msg", sizeof(struct my_msg_struct))) == NULL){
        rtl_printf("mbuff_alloc failed 2\n");
        return -1;
    }
 
    msg->command = STOP_TASK;
 
 
    EXPORT_NO_SYMBOLS;
 
    //ad_init();//ad_$B=i4|2=_(B
    //da_init();
    //counter_init();
    init_dac();
    init_adc();
    cnt_init_module();
    //    outb_p(0x00,DIOBASEADD + 4);
 
    //daconv(0,0x8000);//0[V]_$B$K$9$k_(B
    da_conv(0.0, 0);// for driving motor
    da_conv(0.0, 1);// for driven motor
    da_conv(0.0, 2);// servo off
    da_conv(0.0, 3);// servo off
 
    //create output thread(cycle=0.5kHz)
    pthread_create(&output_task_1, NULL, outputfunc1, (void *)0);
 
    p.sched_priority = 100;
    pthread_setschedparam(output_task_1, SCHED_FIFO, &p);
    pthread_setfp_np(output_task_1, 1);
    pthread_make_periodic_np(output_task_1, gethrtime() + 1000000, 0.1 * 1e9);//_$B$H$j$"$($:<~4|@_Dj!"$N$m$/!#_(B
 
 
    //     outb_p(0x01,DIOBASEADD + 4);
    //    outb_p(0x03,DIOBASEADD + 4);
    da_conv(4.9, 2);// servo on
    da_conv(4.9, 3);// servo on
 
    return 0;
}
 
 
/************************************************/
/* cleanup module                               */
/************************************************/
void cleanup_module(void)
{
    
    pthread_delete_np(output_task_1);
    mbuff_free("data",(void*)shm);//_$B%a%b%j3+J|_(B
    mbuff_free("msg",(void*)msg);
 
    //cleanup_ad();
    //cleanup_da();
    //cleanup_counter();
    da_conv(0.0, 2);// servo off
    da_conv(0.0, 3);// servo off
    cleanup_dac();
    cleanup_adc();
    //    outb_p(0x00,DIOBASEADD + 4);
 
    rtl_printf("\n ***** The module is terminated. *****\n\n");
}