#include <linux/errno.h>
#include <rtl.h>
#include <time.h>
 
#include <rtl_sched.h>
#include <rtl_fifo.h>
 
#include <stdio.h>
#include <sys/io.h>
#include <unistd.h>
#include <stdlib.h>
 
#include <pthread.h>
#include <mbuff.h>
#include <math.h>
 
#include "adc.h"
#include "adda.h"
 
#include "hinf.h"
 
//#define RAND_MAX        2147483647 // this difinition is described in stdlib.h but defined here, because including stdlib.h causes compilation error
 
pthread_t mytask;
 
volatile float * time_resp;
volatile double * cont_real;
volatile struct my_msg_struct * msg;
 
//////////////////////////////////////////////////////////////////////////////
double torq_volt_conv_0(double torq){ 
    double volt;

    volt =  torq / RATED_TORQ * ((double)Pn400_0 / 10);
    
    if(volt > ((double)Pn400_0 / 30)) volt = ((double)Pn400_0 / 30);
    if(volt < -((double)Pn400_0 / 30)) volt = -((double)Pn400_0 / 30);
 
    return volt;
}
double torq_volt_conv_1(double torq){ 
    double volt;

    volt =  torq / RATED_TORQ * ((double)Pn400_1 / 10);
    
    if(volt > ((double)Pn400_1 / 30)) volt = ((double)Pn400_1 / 30);
    if(volt < -((double)Pn400_1 / 30)) volt = -((double)Pn400_1 / 30);
 
    return volt;
}
 
float ang_conv(long counter_data){
    return (float)counter_data / (360 / (float)Pn212 * 1);
}
 
static double
read_theta(int ch)
{
        int d;
 
        cnt_latch();
        cnt_read(&d, 0, ch);
        //return 2.* M_PI/1024. * d;
    return d;
}
////////////////////////////////////////////////////////////////////////////
 
void *thread_code(void *arg)
{
    volatile double * p;
 
    hrtime_t s_time;
    float t, t0;
    int count_record, flag = 0;
 
    int i, j;
    double u, y, w, e1, e2, z, r; 
    double theta_rad[2], theta_rad_before[2];
 
    double x[ORDER_MAX]; // controller states
    double x_tmp[ORDER_MAX];

    double v_i, pv_i, Kp, Ki,tm;
    pv_i = 0;
    Kp = 0.02;
    Ki = 0.03;

    int flag_speed_excess = 0; // 2018.12.19
    
    while(1){
        int ret;
        int err;
 
        ret = pthread_wait_np();
 
        if (msg->command == START_TASK) {
            msg->command = STOP_TASK;
            flag = 1;
            cnt_init_module();
            count_record = 0;
            u = 0.;
            r = (double)COM_ROT_SPEED;;
            theta_rad_before[0] = 0;
            theta_rad_before[1] = 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);
            theta_rad_before[0] = (double)read_theta(0) / (double)Pn212 * 2.0 * M_PI;
            theta_rad_before[1] = (double)read_theta(1) / (double)Pn212 * 2.0 * M_PI;
        }
 
        if(flag == 1){
              
            t = (gethrtime() - s_time)*1.0e-9;

	    w = 0; // disturbance torque for driven motor
	    if((t > 2)&&(t < 3)){
	      w = RATED_TORQ * -0.15;
	    }
	    if((t > 4)&&(t < 5)){
	      w = RATED_TORQ * -0.1 * sin(2*M_PI*5.0 * (t-4.0));
	    }
	    if(flag_speed_excess == 1) w = 0; // 2018.12.20
	    da_conv(torq_volt_conv_1(w), 1);
 
            theta_rad[0] = (double)read_theta(0) / (double)Pn212 * 2.0 * M_PI;
            theta_rad[1] = (double)read_theta(1) / (double)Pn212 * 2.0 * M_PI;
	    y = (theta_rad[0] - theta_rad_before[0]) / msg->sampling_period;
	    z = (theta_rad[1] - theta_rad_before[1]) / msg->sampling_period;
            theta_rad_before[0] = theta_rad[0];
            theta_rad_before[1] = theta_rad[1];

	    //	    if(fabs(z) > SPEED_MAX) flag_speed_excess = 1; // 2018.12.20
	    if((fabs(z) > SPEED_MAX) || (fabs(y) > SPEED_MAX)) flag_speed_excess = 1; // 2018.12.20 further modified after today's lecture

	    e1 = r - y;
	    e2 = y - r;
 
            if(count_record > BUF_LEN * WAITING_SAMPLES){

	      if((t > 0)&&(t < 1)){
		v_i = pv_i + e1 * msg->sampling_period;
		u = Kp * e1 + Ki * v_i;
		pv_i = v_i;
		da_conv(torq_volt_conv_0(u), 0);
	      }

	      tm = Ki * v_i;
	      
	      if(t > 1){
 
                // u = C x + D y
                u = tm;
                p = &cont_real[msg->order * (msg->order + 1)];
                for(i = 0; i < msg->order; i++){
                    u += *p * x[i];
                    p++;
                }
                u += *p * e2;

		if(u > U_MAX) u = U_MAX;
		if(u < -U_MAX) u = -U_MAX;
		if(flag_speed_excess == 1) u = 0; // 2018.12.19
                
#ifndef NO_CONTROL
                da_conv(torq_volt_conv_0(u), 0);
#endif
                
                // 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 * e2;
                    p++;
                }
                memcpy(x, x_tmp, sizeof(x));
	      }
 
            }
            
            if(count_record < BUF_LEN * RECORDING_SAMPLES){
                time_resp[count_record++] = t; 
                time_resp[count_record++] = y; 
                time_resp[count_record++] = z;
		//                time_resp[count_record++] = torq_volt_conv_0(u) / ((double)Pn400_0 / 10)*RATED_TORQ;
		time_resp[count_record++] = u;
                time_resp[count_record++] = w;
            }else{
                flag = 0;
                da_conv(0.0, 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);       // attributes initialization
    pthread_attr_setfp_np(&attr, 1); // set FPU enebled
    sched_param.sched_priority = 1;  // highest priority is set to realtime task
    pthread_attr_setschedparam (&attr, &sched_param);
 
    init_adc();//iwamoto 2017 mod v1
    init_dac();
    cnt_init_module();
    da_conv(0.0, 0);// for driving motor//add iwamoto 2017 mod v1
    da_conv(0.0, 1);// for driven motor//add iwamoto 2017 mod v1
    da_conv(0.0, 2);// servo off//add iwamoto 2017 mod v1
    da_conv(0.0, 3);// servo off//add iwamoto 2017 mod v1
 
    ret = pthread_create (&mytask,  &attr, thread_code, (void *)0);
    pthread_make_periodic_np(mytask, gethrtime(), 0.1 * 1e9);
 
    rtl_printf("********hinf_module: init********\n");
 
 
    da_conv(4.9, 2);// servo on//add iwamoto 2017 mod v1
    da_conv(4.9, 3);// servo on//add iwamoto 2017 mod v1
    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(0.0, 2);// servo off//add iwamoto 2017 mod v1
    da_conv(0.0, 3);// servo off//add iwamoto 2017 mod v1

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