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

from src.OptAlgorithm.AutoConfigClass import AutoConfigClass
from src.OptAlgorithm.ConstantCalculator import ConstantCalculator

class OptTimeCalculator(AutoConfigClass):
    
    params_list = []
    def __init__(self, operator_config : dict, system_config : dict):
        
        cCalculator = ConstantCalculator(operator_config, system_config)
        super().__init__(OptTimeCalculator.params_list, operator_config, system_config, cCalculator.calc())
        self.allTimes = {}
        
    def tGrowNominal(self, F : float) -> float:
        return arcsin(F/(self.Ftogrow)) * sqrt(self.mass_1/self.k_hardness_1)

    def Tclose(self, h1: float, h2: float) -> None:
        v0q = min(sqrt(2 * self.a_max_1 * h1), self.v_max_1)
        v0 = min(v0q, sqrt(1/(self.k_hardness_1*self.mass_1))* self.Ftogrow)
        t1 = v0 / self.a_max_1
        t2t = max(0, (h1 - (self.a_max_1 * t1 * t1 /2)) / v0q)
        T1 = t1 + t2t
        
        t21 = sqrt(h2/self.a_max_2)
        t21 = min(self.v_max_2/self.a_max_2, t21)
        t22 = max(0, (h2 - (self.a_max_2 * t21 * t21)) / self.v_max_2)
        T2 = t22 + 2 * t21
        
        Tclose = max(T1, T2)
        
        tclose_1_acc, tclose_1_speed = self.calcFirstClose(Tclose, h1)
        tclose_2_acc, tclose_2_speed = self.calcSecondClose(Tclose, h2)
            
        self.allTimes["tclose_1_acc"] = tclose_1_acc
        self.allTimes["tclose_1_speed"] = tclose_1_speed
        
        self.allTimes["tclose_2_acc"] = tclose_2_acc
        self.allTimes["tclose_2_speed"] = tclose_2_speed
        self.allTimes["tclose"] = Tclose
    
    def Topen(self, s1 : float, s2 : float, l1 : float, l2 : float, Fs1 : float, Fs2 : float = 0) -> None:
        t11 = sqrt((l1 + Fs1)/self.a_max_1)
        t11 = min(self.v_max_1/self.a_max_1, t11)
        t12 = max(0, ((l1+Fs1) - (self.a_max_1 * t11 * t11)) / self.v_max_1)
        T1 = t12 + 2 * t11
        offset = self.calcSecondOpenOffset(t11, t12, Fs1)
        
        t21 = sqrt(l2/self.a_max_2)
        t21 = min(self.v_max_2/self.a_max_2, t21)
        t22 = max(0, (l2 - (self.a_max_2 * t21 * t21)) / self.v_max_2)
        T2 = t22 + 2 * t21 + offset
        
        topen_1_acc, topen_1_speed = self.calcFirstOpen(T1, l1+Fs1)
        offset = self.calcSecondOpenOffset(topen_1_acc, topen_1_speed, Fs1)
        
        topen_2_acc, topen_2_speed = self.calcSecondOpen(T2 - offset, l2)
        
        self.allTimes["topen_1_acc"] = topen_1_acc    
        self.allTimes["topen_2_offset"] = offset  
        
        self.allTimes["topen_1_acc"] = topen_1_acc
        self.allTimes["topen_1_speed"] = topen_1_speed
        
        self.allTimes["topen_2_acc"] = topen_2_acc
        self.allTimes["topen_2_speed"] = topen_2_speed
        
        if s1 >= l1:
            raise Exception("""S1 >= L1 - недопустимый сценарий,
                            проверьте dist_open_after_1, dist_close_end_1""")
        if s2 >= l2:
            raise Exception("""S2 >= L2 - недопустимый сценарий,
                            проверьте dist_open_after_2, dist_close_end_2""")
            
        s1 += Fs1
        topen_1_mark = sqrt(2 * s1 / self.a_max_1)
        if topen_1_mark > topen_1_acc:
            s1 -= topen_1_acc ** 2 * self.a_max_1 / 2
            v1 = topen_1_acc * self.a_max_1
            if s1 > topen_1_speed * v1:
                s1 -= topen_1_speed * v1
                topen_1_mark = 2*topen_1_acc + topen_1_speed - sqrt(topen_1_acc**2 - 2*s1 / self.a_max_1)
            else:
                topen_1_mark = topen_1_acc + s1 / v1
                
        topen_2_mark = sqrt(2 * s2 / self.a_max_2)
        if topen_2_mark > topen_2_acc:
            s2 -= topen_2_acc ** 2 * self.a_max_2 / 2
            v2 = topen_2_acc * self.a_max_2
            if s2 > topen_2_speed * v2:
                s2 -= topen_2_speed * v2
                topen_2_mark = 2*topen_2_acc + topen_2_speed - sqrt(topen_2_acc**2 - 2*s2 / self.a_max_2)
            else:
                topen_2_mark = topen_2_acc + s2 / v2

        self.allTimes["topen_1_mark"] = topen_1_mark
        self.allTimes["topen_2_mark"] = topen_2_mark
    
    def Tgrow(self) -> None:
        
        v0 = self.allTimes["tclose_1_acc"] * self.a_max_1 
        vF0 = v0 * self.k_hardness_1
        
        vFmax = min(self.v_max_1 * self.k_hardness_1, sqrt(self.k_hardness_1/(self.mass_1))* self.Ftogrow)

        L = sqrt(self.k_hardness_1 / self.mass_1 * self.eff_control ** 2 + vF0*vF0)
        tspeed = sqrt(self.mass_1/self.k_hardness_1) * (arcsin(vFmax / L) - arccos(sqrt(self.k_hardness_1 / self.mass_1) * self.eff_control / L))
        Fspeed = - self.eff_control * cos(self.freq * tspeed) + self.eff_control + 1/self.freq * vF0 * sin(self.freq * tspeed)
        eps = 1e1
        if self.freq**2 * self.Ftogrow**2 - vFmax**2 < -eps:
            raise Exception("""Номинальная траектория набора усилия не может быть достигнута, максимальная скорость превысила скорость траектории
                            , проверьте параметры k_hardness_1, mass_1, k_prop""")
        Fmeet =  1/self.freq * sqrt(self.freq**2 * self.Ftogrow**2 - vFmax**2 + eps)
        Fstart_prop = self.Fstart_prop
        if Fmeet > Fstart_prop:
            raise Exception("""Номинальная траектория набора усилия была достигнута на фазе подпора
                            , проверьте параметры v_max_1, k_prop""")
        tmeet = (Fmeet - Fspeed)/vFmax
        tend = self.tGrowNominal(Fstart_prop) - self.tGrowNominal(Fmeet)
        vp = 1/sqrt(self.k_hardness_1 * self.mass_1) * sqrt(self.Ftogrow**2 - self.Fstart_prop**2)
        ap = Fstart_prop / self.mass_1
        tprop = 2*vp / ap
        
        self.allTimes["tspeed"] = tspeed
        self.allTimes["tmeet"] = tmeet
        self.allTimes["tend"] = tend
        self.allTimes["tprop"] = tprop
        self.allTimes["tgrow"] = tspeed + tmeet + tend + tprop
    
    def T(self, h1 : float, h2 : float, s1 : float, s2 : float, l1 : float, l2 : float) -> dict:
        self.Tclose(h1, h2)
        self.Tgrow()
        self.Topen(s1, s2, l1, l2, self.force_target / self.k_hardness_1, 0)
        return self.allTimes
    
    def Tmovement(self, closeAlgo, tmark) -> None:
        contact = [self.dist_open_start_1 + self.position_start_1, self.dist_open_start_2 + self.position_start_2]
        v0s = []
        pos0s = []
        for i in range(1,3): 
            if tmark < 0:
                raise Exception("""Отрицательное время этапа раскрытия, 
                                проверьте dist_open_after_{1,2}, time_command""")
            v0 = closeAlgo("V"+str(i), "Open", tmark)
            v0s.append(v0)
            x0 = closeAlgo("X"+str(i), "Open", tmark)
            x1 = contact[i-1] - self.__dict__["dist_close_end_"+str(i)]
            x = x1 - x0
            pos0s.append(closeAlgo("X"+str(i), "Open", tmark))
            Tfull = self.time_robot_movement
            
            
            L = self.__dict__["dist_open_end_"+str(i)]
            maxL = contact[i-1] - L - x0
            self.Tmovementi(i, x, Tfull, v0, maxL)
        return pos0s, v0s
            
    def Tmovementi(self, i, Sfull, Tfull, v0, maxL) -> None:
        v0 = abs(v0)
        vmax = self.__dict__["v_max_"+str(i)]
        a = self.__dict__["a_max_"+str(i)]
        t3 = (Tfull + v0 / a) / 2 
        
        sqrtval = a**2 * (a**2 * (Tfull+2*t3)**2 - 8 * a * Sfull + 2 * a* v0 * (Tfull+2*t3) - 3 *v0**2)
        if sqrtval < 0:
            raise Exception("""Невозможно с S_{i} добраться но H*_{i} за указанное время, 
                            проверьте dist_open_after_{i}, dist_close_end_{i}, time_command, time_robot_movement""")
        t1max = ((Tfull+2*t3) + v0/a)/(2) - sqrt(sqrtval) * sqrt(2)/(4*a**2)
        t1 = min(t1max, (vmax- abs(v0))/a)
        t1 = max(0, min(t1, -v0/a + sqrt(v0**2 / (a**2) + (abs(maxL)-v0*v0/a)/a)))
        
        t31 = v0/a + t1
        t5max = (Tfull - v0/a)/2 - t1
        v1 = v0 + a * t1
        S1 = v0*t1 + a*t1*t1/2 + v1*t31 - a*t31*t31/2
        S2max = Sfull + S1
        t5 = min(t5max, (vmax)/a, sqrt(S2max / a))
        t3 = abs(v0)/a + t1 + t5
        t32 = t5
        
        v1 = abs(v0+t1*a)
        v3 = abs(v0 + t1*a - t3*a)
        timeleft = Tfull - t1 - t5 - t3
        sq = -v0*t1 - a*t1**2/2 - v1 * t3 + a*t3**2/2 + v3*t5 - a*t5**2/2
        Sleft = Sfull - sq
        
        t2max = (timeleft - Sleft/v3) / (1 + v1/v3)
        Smovement = -v0 * t1 - a/2 * t1**2 -  v1 * t31 + a/2*t31**2
        t2 = max(0, min(t2max, (abs(maxL) - abs(Smovement))/v1))
        t4 = max(0, Sleft/v3 + v1/v3 * t2)        
        
        tstay = max(0, Tfull - t1 - t2 - t3 - t4 - t5)      
        
        self.allTimes["tmovement_"+str(i)+"_acc"] = t1
        self.allTimes["tmovement_"+str(i)+"_speed"] = t2
        self.allTimes["tmovement_"+str(i)+"_slow"] = t31
        self.allTimes["tmovement_"+str(i)+"_stay"] = tstay
        self.allTimes["tmovement_"+str(i)] = t1 + t2 + t31 + tstay
        self.allTimes["tpreclose_"+str(i)+"_slow"] = t32
        self.allTimes["tpreclose_"+str(i)+"_speed"] = t4
        self.allTimes["tpreclose_"+str(i)+"_acc"] = t5
        self.allTimes["tpreclose_"+str(i)] = t32 + t4 + t5
        T = Tfull
        self.allTimes["tmovement"] = T
        
    def calcFirstClose(self, T : float, s : float) -> tuple[float, float]:
        t1 = T - sqrt(max(0, T**2 - 2 * s / self.a_max_1))
        t1 = min(t1, self.v_max_1 / self.a_max_1)
        t2 = sqrt(max(0, T**2 - 2 * s / self.a_max_1))
        return t1, t2
    
    def calcFirstOpen(self, T : float, s : float) -> tuple[float, float]:
        t1 = T / 2 - sqrt(max(0, T**2 - 4 * s / self.a_max_1)) / 2
        t1 = min(t1, self.v_max_1 / self.a_max_1)
        t2 = sqrt(max(0, T * T - 4 * s / self.a_max_1))
        return t1, t2
        
    def calcSecondOpen(self, T : float, s : float) -> tuple[float, float]:
        t1 = T / 2 - sqrt(max(0, T**2 - 4 * s / self.a_max_2)) / 2
        t1 = min(t1, self.v_max_2 / self.a_max_2)
        t2 = sqrt(max(0, T * T - 4 * s / self.a_max_2))
        return t1, t2
    
    def calcSecondClose(self, T : float, s : float) -> tuple[float, float]:
        t1 = T / 2 - sqrt(max(0, T**2 - 4 * s / self.a_max_2)) / 2
        t1 = min(t1, self.v_max_2 / self.a_max_2)
        t2 = sqrt(max(0, T * T - 4 * s / self.a_max_2))
        return t1, t2
    
    def calcSecondOpenOffset(self, t1 : float, t2 : float, sq : float) -> float:
        s = sq * 1
        offset = sqrt(2 * s / self.a_max_1)
        
        if offset > t1:
            s -= t1 ** 2 * self.a_max_1 / 2
            v1 = t1 * self.a_max_1
            if s > t2 * v1:
                s -= t2 * v1
                
                offset = 2*t1 + t2 - sqrt(t1**2 - 2*s / self.a_max_1)
            else:
                offset = t1 + s / v1
        return offset