import pyb
# 系统默认参数
max_speed = 1  # 电机可达到的最大速度,手动调节
min_speed = 0
max_range = 100 * max_speed # 最大速度*100的范围


class Ps2_Driver:
    '''
    PS2手柄控制驱动
    '''
    def __init__(self, encoder, pid, motor):
        self.enc = encoder
        self.pid = pid
        self.motor = motor
        
    def ps2_driver_init(self):
        # 赋初值
        self.speed = 0

    def ps2_joycontrol(self, joy_data):
        '''
        函数功能：PS2手柄的摇杆控制
        入口参数：ps2手柄摇杆值列表
        返回值 ：PS2手柄摇杆读取到的数值对应的速度
        '''
        global max_speed, min_speed, max_range
        
        angular_vel = (joy_data[5] - 192) * 255 / (192-128) * max_speed # 换算成-100-100 * 速度最大值 = +-速度范围* 100
        linear_vel =  (joy_data[8] - 191) * 255 / (191-128) * max_speed  # 换算成-100-100 * 速度最大值 = +-速度范围* 100
        # linear_vel = linear_vel # 变换成为前进负，后退正
        # angular_vel = -angular_vel # 变换成为左正，右负
        linear_vel = -linear_vel # 变换成为前进正，后退负
        angular_vel = angular_vel # 变换成为左负，右正
        
        # 左右转速度叠加
        if linear_vel > 0: # 前进 + 转向
            if angular_vel > 0: # 右转
                speed_A = linear_vel; speed_B = linear_vel - angular_vel
            elif angular_vel < 0: # 左转
                speed_A = linear_vel + angular_vel; speed_B = linear_vel
            else:
                speed_A = linear_vel; speed_B = linear_vel
            # 速度限幅
            speed_A, speed_B = self.speed_limit(max_range, speed_A, speed_B)
        elif linear_vel < 0: # 后退 + 转向
            if angular_vel > 0: # 右转
                speed_A = linear_vel; speed_B = linear_vel + angular_vel
            elif angular_vel < 0: # 左转
                speed_A = linear_vel - angular_vel; speed_B = linear_vel
            else:
                speed_A = linear_vel; speed_B = linear_vel
            # 速度限幅
            speed_A, speed_B = self.speed_limit(max_range, speed_A, speed_B)
        elif linear_vel == 0:
            if angular_vel > 0: # 右转
                speed_A = angular_vel/4; speed_B = -angular_vel/4
            elif angular_vel < 0: # 左转为负
                speed_A = angular_vel/4; speed_B = -angular_vel/4
            else:
                speed_A = 0; speed_B = 0
            # 速度限幅
            speed_A, speed_B = self.speed_limit(max_range/4, speed_A, speed_B)
        
        return speed_A/100, speed_B/100

    def ps2_keycontrol(self, key):
        '''
        函数功能：使用PS2手柄按键调节速度值，用于测试PID值，调参使用
        入口参数：按键值
        返回值  ：电机速度值
        '''
        global max_range
        # print(key)
        if key == 5: # 上
            self.speed += 5
        elif key == 7: # 下
            self.speed -= 5
        # elif key == 11: # 右前方按钮，停止
        #     self.speed = 0
        # elif key == 12: # 左前方按钮，重启
        #     pyb.hard_reset()

        speed_A, speed_B = self.speed_limit(max_range, self.speed, self.speed)
        return speed_A/100, speed_B/100

    def speed_limit(self, max_range, speed_A, speed_B):
        '''
        函数功能：速度限幅
        入口参数：输入速度
        返回值 ：输出的双轮的运动速度
        '''
        if (speed_A > max_range):
            speed_A = max_range
        elif (speed_A < -max_range): 
            speed_A = -max_range
        if (speed_B > max_range): 
            speed_B = max_range
        elif (speed_B < -max_range): 
            speed_B = -max_range
        return speed_A, speed_B

    def driver(self, target_enc_A, target_enc_B, cur_enc_A, cur_enc_B):
        '''
        函数功能：PS2手柄运动控制
        入口参数：目标双轮编码器值，当前双轮编码器值
        返回值  ：无
        '''
        pwm_A = 0
        pwm_B = 0
        if target_enc_A > 0 or target_enc_B > 0:
            pwm_A = self.pid.pid_A(cur_enc_A, target_enc_A) 
            pwm_B = self.pid.pid_B(cur_enc_B, target_enc_B)
        elif target_enc_A < 0 or target_enc_B < 0:
            pwm_A = self.pid.pid_A(cur_enc_A, target_enc_A) 
            pwm_B = self.pid.pid_B(cur_enc_B, target_enc_B)
        else:
            if target_enc_A == 0:
                pwm_A = self.pid.pid_A(cur_enc_A, 0)  
            if target_enc_B == 0:
                pwm_B = self.pid.pid_B(cur_enc_B, 0)  
        pwm_A, pwm_B = self.motor.direction_control(pwm_A, pwm_B)
        self.motor.motor_run(pwm_A, pwm_B)


class ROS_Driver:
    '''
    ROS控制驱动
    '''
    def __init__(self, encoder, pid, motor):
        self.enc = encoder
        self.pid = pid
        self.motor = motor

    def speed_control(self, angular_vel, linear_vel):
        '''
        函数功能：上位机传入的角速度线速度进行转换
        入口参数：上位机传入的角速度线速度
        返回值  ：双轮实际速度值
        '''
        global max_speed
        speed_A, speed_B = self.enc.anglin_to_speed(angular_vel, linear_vel)
        speed_A, speed_B = self.speed_limit(max_speed, speed_A, speed_B)
        return speed_A, speed_B


    def speed_limit(self, max_range, speed_A, speed_B):
        '''
        函数功能：速度限幅
        入口参数：输入速度
        返回值 ：输出的双轮的运动速度
        '''
        if (speed_A > max_range):
            speed_A = max_range
        elif (speed_A < -max_range): 
            speed_A = -max_range
        if (speed_B > max_range): 
            speed_B = max_range
        elif (speed_B < -max_range): 
            speed_B = -max_range
        return speed_A, speed_B

    def driver(self, target_enc_A, target_enc_B, cur_enc_A, cur_enc_B):
        '''
        函数功能：ROS控制数据
        入口参数：目标双轮编码器值，当前双轮编码器值
        返回值  ：无
        '''
        if target_enc_A > 0 or target_enc_B > 0: 
            pwm_A = self.pid.pid_A(cur_enc_A, target_enc_A) 
            pwm_B = self.pid.pid_B(cur_enc_B, target_enc_B)
        elif target_enc_A < 0 or target_enc_B < 0:
            pwm_A = self.pid.pid_A(cur_enc_A, target_enc_A) 
            pwm_B = self.pid.pid_B(cur_enc_B, target_enc_B)  
        else:
            pwm_A = 0
            pwm_B = 0
        pwm_A = self.pid.pid_A(cur_enc_A, target_enc_A) 
        pwm_B = self.pid.pid_B(cur_enc_B, target_enc_B) 
        pwm_A, pwm_B = self.motor.direction_control(pwm_A, pwm_B)
        self.motor.motor_run(pwm_A, pwm_B)