from OptAlgorithm.PhaseCalc import PhaseCalc
from OptAlgorithm.OptTimeCalculator import OptTimeCalculator
from OptAlgorithm.AutoConfigClass import AutoConfigClass
from OptAlgorithm.ConstantCalculator import ConstantCalculator

from numpy import cos, sin, sqrt, cbrt, arcsin


class OptAlgorithm(AutoConfigClass):
    
    params_list = []

    def __init__(self, operator_config : dict, system_config : dict):
        cCalculator = ConstantCalculator(operator_config, system_config)
        super().__init__(OptAlgorithm.params_list, operator_config, system_config, cCalculator.calc())

        calc = OptTimeCalculator(operator_config, system_config)

        self.Ts = calc.T(self.distance_h_start_1,
                         self.distance_h_start_2,
                         self.distance_s_1,
                         self.distance_s_2,
                         self.distance_l_1,
                         self.distance_l_2)
        
        self.x1_start = self.contact_distance_1 - self.distance_h_start_1
        self.x2_start = self.contact_distance_2 - self.distance_h_start_2
        
        self.x1_contact = self.contact_distance_1
        self.x2_contact = self.contact_distance_2
        
        self.pos0s, self.movementV0s = calc.Tmovement(self.getSpecific, self.getMarkOpen())
        
        self.INF = 1e9

    def getMarkOpen(self):
        return max(self.Ts["topen_2_mark"], self.Ts["topen_1_mark"]) - self.time_command
    
    def V1Close(self, t: float):
        if t < self.Ts["tclose_1_acc"]:
            return self.a_max_1 * t
        else:
            return self.a_max_1 * self.Ts["tclose_1_acc"]
            
    def X1Close(self, t: float):
        t1 = min(t, self.Ts["tclose_1_acc"])
        x0 =  self.a_max_1 * t1 * t1 / 2
        
        t2 = max(t - self.Ts["tclose_1_acc"], 0)
        x1 = self.a_max_1 * self.Ts["tclose_1_acc"] * t2
        return x0 + x1 + self.x1_start
        
    def V2Close(self, t: float):
        if t < self.Ts["tclose_2_acc"]:
            return self.a_max_2 * t
        t -= self.Ts["tclose_2_acc"]
        if t < self.Ts["tclose_2_speed"]:
            return self.a_max_2 * self.Ts["tclose_2_acc"]
        t -= self.Ts["tclose_2_speed"]
        return self.a_max_2 * self.Ts["tclose_2_acc"]  - self.a_max_2 * t
    
    def X2Close(self, t: float):
        t1 = min(t, self.Ts["tclose_2_acc"])
        x0 =  self.a_max_2 * t1 * t1 / 2
        
        t2 = max(min(t - self.Ts["tclose_2_acc"], self.Ts["tclose_2_speed"]), 0)
        x1 = self.a_max_2 * self.Ts["tclose_2_acc"] * t2
        
        t3 = max(min(t - self.Ts["tclose_2_speed"]- self.Ts["tclose_2_acc"], self.Ts["tclose_2_acc"]), 0)
        x2 = self.a_max_2 * self.Ts["tclose_2_acc"] * t3 - self.a_max_2 * t3 * t3 / 2
        
        return x0 + x1 + x2 + self.x2_start
        
    def FClose(self, t: float):
        return 0
        
    def V1Grow(self, t: float):
        F = self.FGrow(t)
        dF0 = self.a_max_1 * self.Ts["tclose_1_acc"] * self.k_hardness_1
    
        dFmax = min(self.v_max_1 * self.k_hardness_1, sqrt(self.k_hardness_1/(self.mass_1))* self.Ftogrow)
        if t < self.Ts["tspeed"]:
            dF = sqrt(self.k_hardness_1 / self.mass_1) * self.eff_control * sin(sqrt(self.k_hardness_1 / self.mass_1) * t) + dF0 * cos(sqrt(self.k_hardness_1 / self.mass_1) * t)
            return dF / self.k_hardness_1
        t -= self.Ts["tspeed"]
        if t < self.Ts["tmeet"]:
            return dFmax / self.k_hardness_1
        t -= self.Ts["tmeet"]
        if t < self.Ts["tend"]:
            return sqrt(self.k_hardness_1/self.mass_1)*sqrt((self.force_target-self.Fprop)**2 - F**2) / self.k_hardness_1
        v0 = sqrt(self.k_hardness_1/self.mass_1)*sqrt((self.force_target-self.Fprop)**2 - self.Fstart_prop**2)
        b = (1/3 * v0 / cbrt((self.force_target - self.Fstart_prop))**2)**3
        dF = 3.0*b*cbrt((F -self.force_target)/b)**2
        return dF / self.k_hardness_1
            
    def X1Grow(self, t: float):
        F = self.FGrow(t)
        x = F / self.k_hardness_1
        return x + self.x1_contact
        
    def V2Grow(self, t: float):
        """
        Считается, что верхний электрод не влияет на набор усилия,
        функция не реализована!, возвращает 0. Устанавливайте kturn = 0
        """
        return 0
    
    def X2Grow(self, t: float):
        """
        Считается, что верхний электрод не влияет на набор усилия,
        функция не реализована!, возвращает 0. Устанавливайте kturn = 0
        """
        return self.x2_contact
        
    def FGrow(self, t: float):
        v0 = self.a_max_1 * self.Ts["tclose_1_acc"]
        dF0 = self.a_max_1 * self.Ts["tclose_1_acc"] * self.k_hardness_1
        
        dFmax = min(self.v_max_1 * self.k_hardness_1, sqrt(self.k_hardness_1/(self.mass_1))* self.Ftogrow)
        eps = 1e1
        self.Fmeet =  1/ self.freq * sqrt(self.freq**2 * self.Ftogrow**2 - dFmax**2 + eps)
        tspeed = self.Ts["tspeed"]
        Fspeed = - self.eff_control * cos(self.freq * tspeed) + self.eff_control + 1/self.freq * dF0 * sin(self.freq * tspeed)
        if t < self.Ts["tspeed"]:
            return - self.eff_control * cos(self.freq * t) + self.eff_control + 1/self.freq * dF0 * sin(self.freq * t)
        t -= self.Ts["tspeed"]
        if t < self.Ts["tmeet"]:
            return Fspeed + t * dFmax
        t -= self.Ts["tmeet"]
        if t < self.Ts["tend"]:
            tm = arcsin(self.Fmeet/(self.Ftogrow)) * 1/self.freq
            return self.Ftogrow * sin(self.freq*(t + tm))
        t -= self.Ts["tend"]
        
        #tprop
        a_max_1 = self.force_target
        v0 = self.freq*sqrt((self.force_target-self.Fprop)**2 - self.Fstart_prop**2)
        b = (1/3 * v0 / cbrt((self.force_target - self.Fstart_prop))**2)**3
        q = self.Fstart_prop
        
        return 3 * t**2 * cbrt(b*b*(q-a_max_1)) + 3 * t * cbrt(q-a_max_1)**2 * cbrt(b) + b * t**3 + q 
    
    def V1Open(self, t: float):
        if t < self.Ts["topen_1_acc"]:
            return -self.a_max_1 * t
        t -= self.Ts["topen_1_acc"]
        if t < self.Ts["topen_1_speed"]:
            return -self.a_max_1 * self.Ts["topen_1_acc"]
        t -= self.Ts["topen_1_speed"]
        if t < self.Ts["topen_1_acc"]:
            return -self.a_max_1 * self.Ts["topen_1_acc"]  + self.a_max_1 * t
        return 0
        
    def X1Open(self, t: float):
        xm = self.force_target / self.k_hardness_1
        t1 = min(t, self.Ts["topen_1_acc"])
        x0 =  -self.a_max_1 * t1 * t1 / 2
        
        t2 = max(min(t - self.Ts["topen_1_acc"], self.Ts["topen_1_speed"]), 0)
        x1 = -self.a_max_1 * self.Ts["topen_1_acc"] * t2
        
        t3 = max(min(t - self.Ts["topen_1_speed"]- self.Ts["topen_1_acc"], self.Ts["topen_1_acc"]), 0)
        x2 = -self.a_max_1 * self.Ts["topen_1_acc"] * t3 + self.a_max_1 * t3 * t3 / 2
        
        return xm + x0 + x1 + x2 + self.x1_contact
        
    def V2Open(self, t: float):
        
        t = max(t-self.Ts["topen_2_offset"] , 0)
        if t < self.Ts["topen_2_acc"]:
            return -self.a_max_2 * t
        t -= self.Ts["topen_2_acc"]
        if t < self.Ts["topen_2_speed"]:
            return -self.a_max_2 * self.Ts["topen_2_acc"]
        t -= self.Ts["topen_2_speed"]
        if t < self.Ts["topen_2_acc"]:
            return -self.a_max_2 * self.Ts["topen_2_acc"]  + self.a_max_2 * t
        return 0
    
    def X2Open(self, t: float):
        t = max(t-self.Ts["topen_2_offset"] , 0)
        t1 = min(t, self.Ts["topen_2_acc"])
        x0 =  -self.a_max_2 * t1 * t1 / 2
        
        t2 = max(min(t - self.Ts["topen_2_acc"], self.Ts["topen_2_speed"]), 0)
        x1 = -self.a_max_2 * self.Ts["topen_2_acc"] * t2
        
        t3 = max(min(t - self.Ts["topen_2_speed"]- self.Ts["topen_2_acc"], self.Ts["topen_2_acc"]), 0)
        x2 = -self.a_max_2 * self.Ts["topen_2_acc"] * t3 + self.a_max_2 * t3 * t3 / 2
        
        return x0 + x1 + x2 + self.x2_contact
        
    def FOpen(self, t: float):
        x1 = self.X1Open(t)
        x2 = self.X2Open(t)
        F = self.k_hardness_1 * max(0, (x1 - self.x1_contact))
        return F
    
    def FMovement(self, t: float):
        x1 = self.X1Movement(t)
        x2 = self.X2Movement(t)
        F = self.k_hardness_1 * max(0, (x1 - self.x1_contact))
        return F
    
    def X1Movement(self, t: float):
        if "x1mov" in self.__dict__.keys():
            return self.x1mov(t)
        self.x1mov = PhaseCalc(cummulative=True)
        xacc = lambda t: self.movementV0s[0] * t - self.a_max_1 * t * t /2
        v1 = self.movementV0s[0] - self.Ts["tmovement_1_acc"] * self.a_max_1
        xspeed = lambda t: v1 * t
        xslow = lambda t: v1 * t + self.a_max_1 * t * t /2
        xstay = lambda t: 0
        xcslow = lambda t: self.a_max_1 * t * t /2
        v3 = (self.Ts["tpreclose_1_slow"]) * self.a_max_1
        xcspeed = lambda t: v3 * t
        xcacc = lambda t: v3 * t - self.a_max_1 * t * t /2
        xend = lambda t: 0
        xstart = lambda t: self.pos0s[0]
        self.x1mov.add_phase(0, xstart)
        self.x1mov.add_phase(self.Ts["tmovement_1_acc"], xacc)
        self.x1mov.add_phase(self.Ts["tmovement_1_speed"], xspeed)
        self.x1mov.add_phase(self.Ts["tmovement_1_slow"], xslow)
        self.x1mov.add_phase(self.Ts["tmovement_1_stay"], xstay)
        self.x1mov.add_phase(self.Ts["tpreclose_1_slow"], xcslow)
        self.x1mov.add_phase(self.Ts["tpreclose_1_speed"], xcspeed)
        self.x1mov.add_phase(self.Ts["tpreclose_1_acc"], xcacc)
        self.x1mov.add_phase(self.INF, xend)
        return self.x1mov(t)
        
    def V1Movement(self, t: float):
        if "v1mov" in self.__dict__.keys():
            return self.v1mov(t)
        self.v1mov = PhaseCalc(cummulative=False)
        xacc = lambda t: self.movementV0s[0] - self.a_max_1 * t
        v1 = self.movementV0s[0] - self.Ts["tmovement_1_acc"] * self.a_max_1
        xspeed = lambda t: v1
        xslow = lambda t: v1 + self.a_max_1 * t
        xstay = lambda t: 0
        xcslow = lambda t: self.a_max_1 * t
        v3 = (self.Ts["tpreclose_1_slow"]) * self.a_max_1
        xcspeed = lambda t: v3
        xcacc = lambda t: v3 - self.a_max_1 * t
        xend = lambda t: 0
        self.v1mov.add_phase(self.Ts["tmovement_1_acc"], xacc)
        self.v1mov.add_phase(self.Ts["tmovement_1_speed"], xspeed)
        self.v1mov.add_phase(self.Ts["tmovement_1_slow"], xslow)
        self.v1mov.add_phase(self.Ts["tmovement_1_stay"], xstay)
        self.v1mov.add_phase(self.Ts["tpreclose_1_slow"], xcslow)
        self.v1mov.add_phase(self.Ts["tpreclose_1_speed"], xcspeed)
        self.v1mov.add_phase(self.Ts["tpreclose_1_acc"], xcacc)
        self.v1mov.add_phase(self.INF, xend)
        return self.v1mov(t)
    
    def X2Movement(self, t: float):
        if "x2mov" in self.__dict__.keys():
            return self.x2mov(t)
        self.x2mov = PhaseCalc(cummulative=True)
        xacc = lambda t: self.movementV0s[1] * t - self.a_max_2 * t * t /2
        v1 = self.movementV0s[1] - self.Ts["tmovement_2_acc"] * self.a_max_2
        xspeed = lambda t: v1 * t
        xslow = lambda t: v1 * t + self.a_max_2 * t * t /2
        xstay = lambda t: 0
        xcslow = lambda t: self.a_max_2 * t * t /2
        v3 = (self.Ts["tpreclose_2_slow"]) * self.a_max_2
        xcspeed = lambda t: v3 * t
        xcacc = lambda t: v3 * t - self.a_max_2 * t * t /2
        xend = lambda t: 0
        xstart = lambda t: self.pos0s[1]
        self.x2mov.add_phase(0, xstart)
        self.x2mov.add_phase(self.Ts["tmovement_2_acc"], xacc)
        self.x2mov.add_phase(self.Ts["tmovement_2_speed"], xspeed)
        self.x2mov.add_phase(self.Ts["tmovement_2_slow"], xslow)
        self.x2mov.add_phase(self.Ts["tmovement_2_stay"], xstay)
        self.x2mov.add_phase(self.Ts["tpreclose_2_slow"], xcslow)
        self.x2mov.add_phase(self.Ts["tpreclose_2_speed"], xcspeed)
        self.x2mov.add_phase(self.Ts["tpreclose_2_acc"], xcacc)
        self.x2mov.add_phase(self.INF, xend)
        return self.x2mov(t)
        
    def V2Movement(self, t: float):
        if "v2mov" in self.__dict__.keys():
            return self.v2mov(t)
        self.v2mov = PhaseCalc(cummulative=False)
        xacc = lambda t: self.movementV0s[1] - self.a_max_2 * t
        v1 = self.movementV0s[1] - self.Ts["tmovement_2_acc"] * self.a_max_2
        xspeed = lambda t: v1
        xslow = lambda t: v1 + self.a_max_2 * t
        xstay = lambda t: 0
        xcslow = lambda t: self.a_max_2 * t
        v3 = (self.Ts["tpreclose_2_slow"]) * self.a_max_2
        xcspeed = lambda t: v3
        xcacc = lambda t: v3 - self.a_max_2 * t
        xend = lambda t: 0
        self.v2mov.add_phase(self.Ts["tmovement_2_acc"], xacc)
        self.v2mov.add_phase(self.Ts["tmovement_2_speed"], xspeed)
        self.v2mov.add_phase(self.Ts["tmovement_2_slow"], xslow)
        self.v2mov.add_phase(self.Ts["tmovement_2_stay"], xstay)
        self.v2mov.add_phase(self.Ts["tpreclose_2_slow"], xcslow)
        self.v2mov.add_phase(self.Ts["tpreclose_2_speed"], xcspeed)
        self.v2mov.add_phase(self.Ts["tpreclose_2_acc"], xcacc)
        self.v2mov.add_phase(self.INF, xend)
        return self.v2mov(t)
    
    def calcPhaseClose(self, t: float) -> tuple[float, float, float, float, float]:
        """
        Получить значения X1, X2, V1, V2, F в момент времени t для фазы смыкания

        Args:
            t (float): Момент времени

        Returns:
            (float, float, float, float, float): X1, X2, V1, V2, F
        """
        return self.X1Close(t), self.X2Close(t), self.V1Close(t), self.V2Close(t), self.FClose(t)
        
    def calcPhaseGrow(self, t: float) -> tuple[float, float, float, float, float]:
        """
        Получить значения X1, X2, V1, V2, F в момент времени t для фазы роста усилия

        Args:
            t (float): Момент времени

        Returns:
            (float, float, float, float, float): X1, X2, V1, V2, F
        """
        return self.X1Grow(t), self.X2Grow(t), self.V1Grow(t), self.V2Grow(t), self.FGrow(t)
        
    def calcPhaseOpen(self, t: float) -> tuple[float, float, float, float, float]:
        """
        Получить значения X1, X2, V1, V2, F в момент времени t для фазы раксрытия

        Args:
            t (float): Момент времени

        Returns:
            (float, float, float, float, float): X1, X2, V1, V2, F
        """
        return self.X1Open(t), self.X2Open(t), self.V1Open(t), self.V2Open(t), self.FOpen(t)
    
    def calcPhaseMovement(self, t: float) -> tuple[float, float, float, float, float]:
        """
        Получить значения X1, X2, V1, V2, F в момент времени t для фазы раксрытия

        Args:
            t (float): Момент времени

        Returns:
            (float, float, float, float, float): X1, X2, V1, V2, F
        """
        return self.X1Movement(t), self.X2Movement(t), self.V1Movement(t), self.V2Movement(t), self.FMovement(t)
    
    def getSpecific(self, param : str, phase : str, t : float) -> float:
        """
        Получить значение величины в определенную фазу в момент времени t (с начала фазы)

        Args:
            param (str): Значение из списка X1 | X2 | V1 | V2 | F
            phase (str): Значение из списка: Close | Grow | Open | Movement
            t (float): Время

        Returns:
            Значение величины
        """
        funcName = param + phase
        try:
            func = getattr(self, funcName)
        except:
            print("Wrong param or phase name")
            return 0
        return func(t)
    
    def getVar(self, param : str, t : float) -> float:
        """
        Получить значение величины в момент времени t (БЕЗ УЧЕТА СВАРКИ!)

        Args:
            param (str): Значение из списка X1 | X2 | V1 | V2 | F
            t (float): Время

        Returns:
            Значение величины
        """
        if t < self.Ts["tclose"]:
            return  self.getSpecific(param, "Close", t)
        t -= self.Ts["tclose"]
        if t < self.Ts["tgrow"] :
            return  self.getSpecific(param, "Grow", t)
        t -= self.Ts["tgrow"]
        tmark = self.getMarkOpen()
        if t < tmark:
            return  self.getSpecific(param, "Open", t)
        t -= tmark
        if t < self.Ts["tmovement"] :
            return  self.getSpecific(param, "Movement", t)
        return self.getSpecific(param, "Movement", t)

