/*
	滑膜面调参目标使滑模面s趋近于0，趋近率选择：
	指数趋近率
	幂次趋近率
	
	J给个固定的值，乘一个输出，J的大小取决于控制量受噪声影响
	IMU噪声对其影响很大，J应该给大些，并且K和s不能给太大，如果基于编码器控制J建议直接给1
	ε(epsilon)不宜过大，过大会影响精度,limit和ε给的值决定线性区间
	K是增益，K大了也会抖,并且影响到目标位置是否能停下来，K太大会停不下来
	c决定s趋近0的快满程度，c大了会抖
	
	注意，滑膜控制是位置控制，这个位置变化是在控制频率内的，我的速度也要乘这个时间如（0.002）
	否则速度误差占比会非常大，电机停不下来
	输出u
	
	参考资料
	https://zhuanlan.zhihu.com/p/78549442  

*/
#include "Sliding.h"
#include "math.h"
#include "user_math.h"

/**
 * @brief 参数调整时使其连续
 *
 * @param Sliding s结构体
 */
static void OutContinuation(Sliding *s){
	
    if(s->param->K != 0 && s->param->c2 != 0)
    {
        s->position.error_integral = (s->param_last->K / s->param->K) * (s->param_last->c2 / s->param->c2) * s->position.error_integral;
        s->velocity.error_integral = (s->param_last->K / s->param->K) * (s->param_last->c2 / s->param->c2) * s->velocity.error_integral;
    }
    s->param_last = s->param;
}

/**
 * @brief 符号函数
 *
 * @param  s 滑膜面
 * @return 
 */
static float Signal(float s){
    if (s > 0)
        return 1;
    else if (s == 0)
        return 0;
    else
        return -1;
}

/**
 * @brief 饱和函数
 *滑模控制中用于代替符号函数的连续化近似函数，用于抑制滑模控制的高频抖振现象。
	函数特性：
	在滑模面 s 较小时（边界层内），输出与输入呈线性关系
	在滑模面 s 较大时（边界层外），输出饱和为 ±1
	连续可导（在边界点一阶连续），减少控制系统的高频切换
 * @param  s 滑膜面
 * @param  sliding 滑膜结构体
 * @return Signal(y)/y
 */
static float Sat(float s,Sliding *sliding){
    float y;
    y = s / sliding->param->epsilon;
    if (fabs(y) <= sliding->param->limit)
        return y;
    else
        return Signal(y);
}


/**
 * @brief 滑膜设立参数
 *
 * @param Sliding s结构体
 * @param mode 滑膜模式
 * @param param 滑膜参数
 * @return 
 */
static void SMC_SetParam(Sliding *s,Smc_mode mode,SlidingParam *param) {
	///EXPONENT,POWER,VELSMC 参数设定
		 s->mode = mode;
	switch(mode){	
		case EXPONENT:
		case POWER:
		case VELSMC:	
			s->param = param;
    OutContinuation(s);
		break;

		case TFSMC:		
		break;
    case EISMC:
				s->param = param;	

    OutContinuation(s);

		break;	
	}
}

/**
 * @brief 滑膜重置
 *
 * @param Sliding s结构体
 * @return 
 */
void SMC_Reset(Sliding *s){
		/* 位置速度清零 */
	  s->position.tar_now = 0;
    s->position.tar_last = 0;
    s->position.tar_differential = 0;
    s->velocity.tar_now = 0;
    s->velocity.tar_last = 0;
    s->velocity.tar_differential = 0;
		/* 误差清零 */
    s->position.error = 0;
    s->velocity.error = 0;

    s->position.error_eps = 0;
    s->velocity.error_eps = 0;
    s->position.now_get = 0;
    s->velocity.now_get = 0;
		/* 一阶导清零 */
		s->velocity.tar_differential_last=0;
		s->position.tar_differential_second=0;
		s->position.tar_differential_last=0;	
		/* 积分清零 */
		s->position.error_integral=0;	
    s->velocity.error_integral = 0;	
}


/**
 * @brief 滑膜初始化，设立参数
 *
 * @param Sliding s结构体
 * @param mode 滑膜模式
 * @param param 滑膜参数
 * @return 
 */
void SMC_Init(Sliding *s,Smc_mode mode,SlidingParam *param){
	
    s->J = 0;
    s->param->K = 0;
    s->param->c = 0;
    s->param->epsilon = 0;
    s->param->u_max = 0;
    s->param->limit = 0;

    s->position.tar_now = 0;
    s->position.tar_last = 0;
    s->position.tar_differential = 0;

    s->position.error = 0;
    s->velocity.error = 0;
    s->velocity.error_integral = 0;
    s->velocity.error_integral_max = 0;
    s->position.error_eps = 0;
    s->velocity.error_eps = 0;
    s->position.now_get = 0;
    s->velocity.now_get = 0;
	
		/* 设立参数 */
		SMC_SetParam(s,mode,param);
	
}


/**
 * @brief 位置环速度更新
 *
 * @param Sliding s结构体
 * @param target 目标位置
 * @param pos_now 现在位置
 * @param vol_now 现在速度
 * @param dt 采样频率
 * @return 
 */
void SMC_PErrorUpdate(Sliding *s,float target, float pos_now, float vol_now,float dt) {
    s->position.tar_now = target;
		/* 一阶导(速度) */
    s->position.tar_differential = (float)((s->position.tar_now - s->position.tar_last)/dt);
		/* 二阶导 */
		s->position.tar_differential_second=(float)((s->position.tar_differential- s->position.tar_differential_last)/dt);
		
    s->position.error = pos_now - target;
    s->velocity.error = vol_now - s->position.tar_differential;
		/* 更新上一次的值 */
    s->position.tar_last = s->position.tar_now;
		/* 位置误差积分项 */
    s->position.error_integral += (float)(s->position.error * dt); 
		/* 二阶导更新 */
    s->position.tar_differential_last = s->position.tar_differential; 

}

/**
 * @brief 速度环误差更新
 *
 * @param Sliding s结构体
 * @param target 目标位置
 * @param vol_now 现在速度
 * @param dt 采样频率
 * @return 
 */
void SMC_VErrorUpdate(Sliding *s,float target,float vol_now,float dt) {
    
		s->velocity.tar_now = target;
    s->velocity.tar_differential = (float)((s->velocity.tar_now - s->velocity.tar_last)/dt);

//    smc.error.tar_differential_second = (float)((smc.error.tar_differential- smc.error.tar_differential_last)/SAMPLE_PERIOD); ///二阶导

    s->velocity.error = vol_now - s->velocity.tar_now;
		/* 速度误差积分项 */
    s->velocity.error_integral += (float)(s->velocity.error * dt); 
//    if(std::fabs(smc.error.v_error_integral) > smc.error.v_error_integral_max) //积分限幅
//    {
//        smc.error.v_error_integral = smc.error.v_error_integral_max;
//    }
    s->velocity.tar_last = s->velocity.tar_now;


}


//void SMC_Integval_Reset()
//{
//    smc.error.v_error_integral = 0;
//    smc.error.p_error_integral = 0;
//}

/**
 * @brief 滑膜计算
 *
 * @param Sliding s结构体
 * @param mode 滑膜模式
 * @return u 输出电流
 */
float Smc_Calc(Sliding *s){
		/* 输出，滑模面 */
    float u,fun;
		/* tfsmc 位置 幂 (这个是移值的，放在这待开发)*/
		static float pos_pow;
		/* 力矩 */
		s->fun=fun;
    switch (s->mode) {
        case EXPONENT:///线性滑模面，指数趋近率

            if (fabs(s->position.error) - s->position.error_eps < 0){
                s->position.error = 0;
                return 0;
            }
						/* 建立滑模面 */
            s->s = s->param->c * s->position.error + s->velocity.error; 
						/* 饱和函数消除抖动 */						
            fun = Sat(s->s,s);
//            u =  smc.param.J * ( (-smc.param.c * smc.error.v_error) - smc.param.K * smc.s - smc.param.epsilon * fun + smc.error.tar_differential_second); //控制器计算,指数趋近率
						/* 控制器计算,指数趋近率  */
            u = s->param->J * ( (-s->param->c * s->velocity.error) - s->param->K * s->s - s->param->epsilon * fun); 
						s->s1=-s->param->c * s->velocity.error;
						s->s2=s->param->K * s->s;
						s->s3=s->param->epsilon * fun;
						s->s4=fun;
            break;
        case POWER:///线性滑模面，幂次趋近率

            if (fabs(s->position.error) - s->position.error_eps < 0){
                s->position.error = 0;
                return 0;
            }

            s->s = s->param->c * s->position.error + s->velocity.error; //滑模面
            fun = Sat(s->s,s);//饱和函数消除抖动
//            u =  smc.param.J * ( (-smc.param.c * smc.error.v_error) - smc.param.K * smc.s - smc.param.K * (std::pow(std::fabs(smc.s),smc.param.epsilon)) * fun + smc.error.tar_differential_second); //控制器计算,幂次趋近率
            u =  s->J * ( (-s->param->c * s->velocity.error) - s->param->K * s->s - s->param->K * (pow(fabs(s->s),s->param->epsilon)) * fun); //控制器计算,幂次趋近率

            break;
        case TFSMC:///tfsmc

            if (fabs(s->position.error) - s->position.error_eps < 0){
                s->position.error = 0;
                return 0;
            }

            pos_pow = pow(fabs(s->position.error),s->param->q/s->param->p);
            if(s->position.error<=0) pos_pow = -pos_pow;


            s->s = s->param->beta * pos_pow + s->velocity.error; //滑模面
						/* 饱和函数消除抖动 */			
            fun = Sat(s->s,s);

            if(s->position.error!=0){
                u = s->J * (s->position.tar_differential_second//目标值的二阶导 暂定是否删除，需测试
                             -s->param->K * s->s //s*K
                             -s->param->epsilon * fun  //epsilon*SAT(S)
                             -s->velocity.error * ((s->param->q * s->param->beta) * pos_pow) / (s->param->p * s->position.error)); //控制器计算
            }
            else u = 0;
            break;
        case VELSMC:///比例积分滑模面，指数趋近律，速度控制
						/* 滑模面 */
            s->s = s->velocity.error + s->param->c * s->velocity.error_integral; 
            fun = Sat(s->s,s);
				
						/*控制器计算，速度控制*/
            u =  s->J * (s->velocity.tar_differential - (s->param->c * s->velocity.error) - s->param->K * s->s - s->param->epsilon * fun); 
            break;

        case EISMC:///比例积分滑模面，指数趋近律，位置控制
            if (fabs(s->position.error) - s->position.error_eps < 0){
                s->position.error = 0;
                return 0;
            }
						/* 滑模面 */
            s->s = s->param->c1 * s->position.error + s->velocity.error + s->param->c2 * s->position.error_integral; 
            fun = Sat(s->s,s);
						/* 控制器计算,指数趋近率 */
            u =  s->J * ( (-s->param->c1 * s->velocity.error)- s->param->c2 * s->position.error - s->param->K * s->s - s->param->epsilon * fun); 
            break;
    }
		/* 更新上一步的误差 */
    s->position.error_last = s->position.error; 

    //控制量限幅
    if (u > s->param->u_max)
    {
        u = s->param->u_max;
    }
    if (u < -s->param->u_max)
    {
        u = -s->param->u_max;
    }
    s->u = u;
    return u;
}