#!/usr/bin/env python
# -*- coding: utf-8 -*-
'''
# @Time    : 2025/2/18 10:06
# @Author  : hjw
# @File    : controlAlgor.py
'''

from abc import ABC, abstractmethod


class ControlAlgorithm(ABC):
    @abstractmethod
    def calculate_speed(self, current, target):
        pass


class PIDAlgorithm(ControlAlgorithm):
    def __init__(self, kp=0.5, ki=0.01, kd=0.1):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.integral = 0
        self.last_error = 0
        self.min_speed = 1  # 最小运行速度
        self.max_speed = 100  # 最大运行速度

    def calculate_speed(self, current, target):
        error = target - current
        self.integral += error
        derivative = error - self.last_error
        self.last_error = error

        # 抗积分饱和
        if self.integral > 1000:
            self.integral = 1000
        elif self.integral < -1000:
            self.integral = -1000

        output = self.kp * error + self.ki * self.integral + self.kd * derivative
        #误差5g     5*0.5   + 1000* 0.01 + [0.5*5]  = 2.5 + 10 + ... = 12.5
        #误差500g   500*0.5 + 1000* 0.01 + [0.5*10] = 250 + 10 + ... = 260
        return max(self.min_speed, min(self.max_speed, output)), error             #


class FuzzyLogicAlgorithm(ControlAlgorithm):
    def calculate_speed(self, current, target):
        error = target - current
        abs_error = abs(error)

        if abs_error > 100:
            return 100
        elif abs_error > 50:
            return 70
        elif abs_error > 20:
            return 50
        else:
            return 30