# -*- coding: utf-8 -*-
"""
Created on Sat Feb 11 16:13:56 2023

@author: smole
"""

import math
import numpy as np
from scipy.optimize import fsolve
from matplotlib import pyplot as plt
import matplotlib as mpl
from matplotlib.animation import FuncAnimation
import scipy as sp
from scipy.signal import butter, filtfilt



fileloc="pokus1.txt"
input_data=np.loadtxt(fileloc,skiprows=2)

pocet_dat=250#250
zvednuti_test=input_data[:pocet_dat,3]+np.abs(input_data[0,3])
zvednuti_test=sp.ndimage.gaussian_filter(zvednuti_test, sigma=15)


cas_test=input_data[:pocet_dat,0]/15
cas_test1=cas_test[:np.size(zvednuti_test)-2]

def omega(zvednuti,cas):
    omega=[]
    size=np.size(zvednuti)
    zvednuti=zvednuti*np.pi/180
    for i in range(size-1):
        omega_temp=(zvednuti[i+1]-zvednuti[i])/(cas[i+1]-cas[i])
        omega.append(omega_temp)
        
    return omega

def alfa(omega_,cas):
    alfa=[]
    size=np.size(omega_)
    for i in range(size-1):
        alfa_temp=(omega_[i+1]-omega_[i])/(cas[i+1]-cas[i])
        alfa.append(alfa_temp)
        
    return alfa       
    
omega_2=omega(zvednuti_test,cas_test)    
alfa_2=alfa(omega_2,cas_test) 

zvednuti_test=zvednuti_test[:pocet_dat-2]
omega_2=omega_2[:pocet_dat-1]

    
def vyber_ANSYS(vector):
    selected_elements = vector[::15]
    return selected_elements



res=np.size(zvednuti_test)

cas=np.linspace(0,5,res)
fi2_=zvednuti_test[-1]/cas[-1]



L_1=0.135
L_2=1.354
L_3=0.135
L_4=1.350
L_5=0.32947
L_6=1.471
L_7=0.343
L_8=1.595
L_9=0.222

L_10=0.246
L_11=0.219
L_12=0.155


def deg2rad(fi):
    return fi*np.pi/180

fi_1=deg2rad(120)
fi_2=deg2rad(344)
fi_3=deg2rad(298)
fi_4=deg2rad(164)
fi_5=deg2rad(89)
fi_6=deg2rad(107)
fi_7=deg2rad(79)
fi_8=deg2rad(286)
fi_9=deg2rad(251)
fi_r1=deg2rad(133)

fi_10=deg2rad(300)
fi_11=deg2rad(255)
fi_12=deg2rad(146) 
#beta=deg2rad(122)

#omega_2=0.1
#alfa_2=0.1

#souřadnice uzlových bodů
A=[-0.0682, 0.1165]
B=[1.2337, -0.2556]
C=[1.6786, -1.7874]
D=[1.3054,-0.0455]
E=[1.2969, -0.3748]
H=[1.7425, -1.4504]
G=[0,0]
f=[0.9445,-0.4107]
Z=[2.5370203,-0.9158923]#nová zátěž
P=[0.093,-0.2897]

I=[1.8648,-1.6638]
J=[1.8065,-1.8749]

#moment setrvačnosti k počátku souřadnicového systému 
i28=123.4237955
i39=0.367521978
i4=7.882636938
i6=10.68219815
i7=0.234545468
i10=0.098543458
i11=1.7142708

#souřadnice těžiště
T_28=[1.1134, -0.6922]
T_39=[1.2572, -0.2196]
T_4=[0.6579, -0.164]
T_6=[1.5473, -0.8228]
T_7=[1.6425, -1.6059]
T_10=[1.8092,-1.5671]
T_11=[1.8874,-1.6615]

#hmotnosti v kg
m28=183.2
m39=26.8 #m35
m4=47.6
m6=66.4
m7=14.8
m10=10.6
m11=62

#tíhové síly
m_zatez=1800
g=9.81
FG28=m28*g
FG4=m4*g
FG6=m6*g
FG7=m7*g
FG39=m39*g
FG10=m10*g
FG11=m11*g
FGzatez=m_zatez*g

def func(x,fi_2i):
    return [L_1* np.cos(fi_1) + L_2* np.cos(fi_2i) + L_3* np.cos(x[0]) + L_4* np.cos(x[1]),
            L_1* np.sin(fi_1) + L_2* np.sin(fi_2i) + L_3* np.sin(x[0]) + L_4* np.sin(x[1])]

def func2(x, fi_9i, fi_8i):
    return [L_9* np.cos(fi_9i) + L_6* np.cos(x[0]) + L_7* np.cos(x[1]) + L_8* np.cos(fi_8i),
            L_9* np.sin(fi_9i) + L_6* np.sin(x[0]) + L_7* np.sin(x[1]) + L_8* np.sin(fi_8i)]

def func3(x, fi_3i):
    return [L_3* np.cos(fi_3i) + L_9* np.cos(x[0]) + L_5* np.cos(x[1]),
            L_3* np.sin(fi_3i) + L_9* np.sin(x[0]) + L_5* np.sin(x[1])]

def func4(x, fi_7i, fi_12i):
    return [L_7* np.cos(fi_7i) + L_12* np.cos(fi_12i) + L_10* np.cos(x[0]) + L_11* np.cos(x[1]),
            L_7* np.sin(fi_7i) + L_12* np.sin(fi_12i) + L_10* np.sin(x[0]) + L_11* np.sin(x[1])]

def pol2cart (r, fi):
    x=r*np.cos(fi)
    y=r*np.sin(fi)
    return (x,y)

def pol2cartE (r, fi):
    x=kv_zna_E(kvadrant(fi))[0]*r*np.cos(fi)
    y=kv_zna_E(kvadrant(fi))[1]*r*np.sin(fi)
    return (x,y)


def polo_Z(zvednuti, omega_2, alfa_2):
    angle_rad = kin_par(zvednuti, omega_2, alfa_2)[9,0] - vni_uh_J([kin_vel_J(zvednuti, omega_2, alfa_2)[0],kin_vel_C(zvednuti, omega_2, alfa_2)[0],kin_vel_H(zvednuti, omega_2, alfa_2)[0],kin_vel_I(zvednuti, omega_2, alfa_2)[0]]) - math.radians(22)
    distance=vzd_bodu(J, Z)
    # výpočet nových souřadnic
    x_new = J[0] + distance * math.cos(angle_rad)
    y_new = J[1] + distance * math.sin(angle_rad)
    
    return x_new, y_new

def vni_uh_J(souradnice):
    uhly = []
    for i in range(4):
        x1, y1 = souradnice[i]
        x2, y2 = souradnice[(i + 1) % 4]
        x3, y3 = souradnice[(i + 2) % 4]
        vektor1 = [x1 - x2, y1 - y2]
        vektor2 = [x3 - x2, y3 - y2]
        skalarni_soucin = vektor1[0] * vektor2[0] + vektor1[1] * vektor2[1]
        velikost_vektoru1 = math.sqrt(vektor1[0] ** 2 + vektor1[1] ** 2)
        velikost_vektoru2 = math.sqrt(vektor2[0] ** 2 + vektor2[1] ** 2)
        uhel_rad = math.acos(skalarni_soucin / (velikost_vektoru1 * velikost_vektoru2))    
        uhly.append(uhel_rad)
    return uhly[3]

def kvadrant(angle):
    while angle < 0:
        angle += 2*math.pi
    while angle >= 2*math.pi:
        angle -= 2*math.pi
    if angle >= 0 and angle < math.pi/2:
        return 1
    elif angle >= math.pi/2 and angle < math.pi:
        return 2
    elif angle >= math.pi and angle < 3*math.pi/2:
        return 3
    elif angle >= 3*math.pi/2 and angle < 2*math.pi:
        return 4
    else:
        return None # pokud úhel není v rozmezí 0 až 2π

def kv_zna_E(quadrant):
    if quadrant == 1:
        return [1,1]
    elif quadrant == 2:
        return [-1,-1]
    elif quadrant == 3:
        return [-1,-1]
    elif quadrant == 4:
        return [1,1]


def vector(A,B):
    vx = B[0] - A[0] #absolutní hodnota?
    vy = B[1] - A[1]
    return [vx, vy]

def vzd_bodu(A, B):
    x=math.sqrt((B[0]-A[0])**2 + (B[1]-A[1])**2)
    return x


def uhly(a, b, c):
    # výpočet úhlu alfa
    cos_alfa = (b**2 + c**2 - a**2) / (2*b*c)
    alfa = math.acos(cos_alfa)

    # výpočet úhlu beta
    cos_beta = (a**2 + c**2 - b**2) / (2*a*c)
    beta = math.acos(cos_beta)

    # výpočet úhlu gama
    cos_gama = (a**2 + b**2 - c**2) / (2*a*b)
    gama = math.acos(cos_gama)

    return alfa, beta, gama

def prepocet_poloha(bod_otaceni, koncovy_bod, zvednuti):
    vektor = [koncovy_bod[0] - bod_otaceni[0], koncovy_bod[1] - bod_otaceni[1]]
    radius = math.sqrt(vektor[0]**2 + vektor[1]**2)
    uhel=np.arctan2(vektor[1],vektor[0])
    degree = deg2rad(zvednuti)+uhel  
    x = radius * math.cos(degree)
    y = radius * math.sin(degree)
    x1 = x + bod_otaceni[0]
    y1 = y + bod_otaceni[1]
    return [x1, y1]

def steiner(I, XY, m):
    r = np.sqrt(XY[0]**2 + XY[1]**2)
    It = I + m * r**2
    return It

def kin_vel_B(zvednuti, omega_2, alfa_2):
    fi_2i=fi_2+deg2rad(zvednuti)
    x3,y3=pol2cart(L_3,kin_par(zvednuti, omega_2, alfa_2)[0,0])
    x4,y4=pol2cart(L_4,kin_par(zvednuti, omega_2, alfa_2)[1,0])
    vX2=-omega_2*L_2*np.sin(fi_2i)
    vY2=omega_2*L_2*np.cos(fi_2i)
    aX2=-L_2*np.cos(fi_2i)*omega_2**2-L_2*np.sin(fi_2i)*alfa_2
    aY2=-L_2*np.sin(fi_2i)*omega_2**2+L_2*np.cos(fi_2i)*alfa_2
    return np.array([[-x4-x3,-y4-y3],[vX2,vY2],[aX2,aY2]])


def kin_vel_E(zvednuti, omega_2, alfa_2):
    #fi_2i=fi_2+deg2rad(zvednuti)
    kv=kin_par(zvednuti, omega_2, alfa_2)
    fi_4=kv[1,0]
    omega_4=kv[1,1]
    alfa_4=kv[1,2]
    x4,y4=pol2cart(L_4,fi_4)
    vX4=-omega_4*L_4*np.sin(fi_4)
    vY4=omega_4*L_4*np.cos(fi_4)
    aX4=-L_4*np.cos(fi_4)*omega_4**2-L_4*np.sin(fi_4)*alfa_4
    aY4=-L_4*np.sin(fi_4)*omega_4**2+L_4*np.cos(fi_4)*alfa_4
    return np.array([[-x4,-y4],[-vX4,-vY4],[-aX4,-aY4]])

def kin_vel_C(zvednuti, omega_2, alfa_2):
    fi_2i=fi_2+deg2rad(zvednuti)
    x1,y1=pol2cart(L_1,fi_1)
    x2,y2=pol2cart(L_2,fi_2i)
    B=kin_vel_B(zvednuti, omega_2, alfa_2)
    kv=kin_par(zvednuti, omega_2, alfa_2)
    fi_8=kv[5,0]
    omega_8=kv[5,1]
    alfa_8=kv[5,2]
    x8,y8=pol2cart(L_8,fi_8)
    vX8=-omega_8*L_8*np.sin(fi_8)
    vY8=omega_8*L_8*np.cos(fi_8)
    aX8=-L_8*np.cos(fi_8)*omega_8**2-L_8*np.sin(fi_8)*alfa_8
    aY8=-L_8*np.sin(fi_8)*omega_8**2+L_8*np.cos(fi_8)*alfa_8
    return np.array([[x1+x2+x8,y1+y2+y8],[vX8+B[1,0],vY8+B[1,1]],[aX8+B[2,0],aY8+B[2,1]]])

def kin_vel_H(zvednuti, omega_2, alfa_2):    
    C=kin_vel_C(zvednuti, omega_2, alfa_2)
    kv=kin_par(zvednuti, omega_2, alfa_2)
    fi_7=kv[4,0]
    omega_7=kv[4,1]
    alfa_7=kv[4,2]
    x7,y7=pol2cart(L_7,fi_7)
    vX7=-omega_7*L_7*np.sin(fi_7)
    vY7=omega_7*L_7*np.cos(fi_7)
    aX7=-L_7*np.cos(fi_7)*omega_7**2-L_7*np.sin(fi_7)*alfa_7
    aY7=-L_7*np.sin(fi_7)*omega_7**2+L_7*np.cos(fi_7)*alfa_7
    return np.array([[x7+C[0,0],y7+C[0,1]],[vX7+C[1,0],vY7+C[1,1]],[aX7+C[2,0],aY7+C[2,1]]])

def kin_vel_D(zvednuti, omega_2, alfa_2):    
    B=kin_vel_B(zvednuti, omega_2, alfa_2)
    kv=kin_par(zvednuti, omega_2, alfa_2)
    fi_9=kv[6,0]
    omega_9=kv[6,1]
    alfa_9=kv[6,2]
    x9,y9=pol2cart(L_9,fi_9)
    vX9=-omega_9*L_9*np.sin(fi_9)
    vY9=omega_9*L_9*np.cos(fi_9)
    aX9=-L_9*np.cos(fi_9)*omega_9**2-L_9*np.sin(fi_9)*alfa_9
    aY9=-L_9*np.sin(fi_9)*omega_9**2+L_9*np.cos(fi_9)*alfa_9
    return np.array([[-x9+B[0,0],-y9+B[0,1]],[vX9+B[1,0],vY9+B[1,1]],[aX9+B[2,0],aY9+B[2,1]]])

def kin_vel_T28(zvednuti, omega_2, alfa_2):    
    fi_T28=fi_2+deg2rad(zvednuti)-uhly(L_2, vzd_bodu(A,T_28), vzd_bodu(B, T_28))[2]
    x1,y1=prepocet_poloha(A, T_28, zvednuti)
    L_28=vzd_bodu(A,T_28)
    vX28=-omega_2*L_28*np.sin(fi_T28)
    vY28=omega_2*L_28*np.cos(fi_T28)
    aX28=-L_28*np.cos(fi_T28)*omega_2**2-L_28*np.sin(fi_T28)*alfa_2
    aY28=-L_28*np.sin(fi_T28)*omega_2**2+L_28*np.cos(fi_T28)*alfa_2
    return np.array([[x1,y1],[vX28,vY28],[aX28,aY28]])

def kin_vel_T7(zvednuti, omega_2, alfa_2):
    kv=kin_par(zvednuti, omega_2, alfa_2)
    fi_7=kv[4,0]
    omega_7=kv[4,1]
    alfa_7=kv[4,2]    
    C_=kin_vel_C(zvednuti, omega_2, alfa_2)
    fi_T7=fi_7+uhly(L_7, vzd_bodu(C,T_7), vzd_bodu(H, T_7))[2]
    L_T7=vzd_bodu(C,T_7)
    x_T7,y_T7=pol2cart(L_T7,fi_T7)    
    vXT7=-omega_7*L_T7*np.sin(fi_T7)
    vYT7=omega_7*L_T7*np.cos(fi_T7)
    aXT7=-L_T7*np.cos(fi_T7)*omega_7**2-L_T7*np.sin(fi_T7)*alfa_7
    aYT7=-L_T7*np.sin(fi_T7)*omega_7**2+L_T7*np.cos(fi_T7)*alfa_7
    return np.array([[x_T7+C_[0,0],y_T7+C_[0,1]],[vXT7+C_[1,0],vYT7+C_[1,1]],[aXT7+C_[2,0],aYT7+C_[2,1]]])

def kin_vel_T4(zvednuti, omega_2, alfa_2): 
    kv=kin_par(zvednuti, omega_2, alfa_2)
    fi_4=kv[1,0]
    omega_4=kv[1,1]
    alfa_4=kv[1,2]
    E_=kin_vel_E(zvednuti, omega_2, alfa_2)
    fi_T4=fi_4-uhly(L_4, vzd_bodu(E,T_4), vzd_bodu(G, T_4))[2]
    x_T4,y_T4=prepocet_poloha(G, T_4, zvednuti)
    L_T4=vzd_bodu(E,T_4)
    vXT4=-omega_4*L_T4*np.sin(fi_T4)
    vYT4=omega_4*L_T4*np.cos(fi_T4)
    aXT4=-L_T4*np.cos(fi_T4)*omega_4**2-L_T4*np.sin(fi_T4)*alfa_4
    aYT4=-L_T4*np.sin(fi_T4)*omega_4**2+L_T4*np.cos(fi_T4)*alfa_4
    return np.array([[x_T4,y_T4],[vXT4+E_[1,0],vYT4+E_[1,1]],[aXT4+E_[2,0],aYT4+E_[2,1]]])

def kin_vel_T6(zvednuti, omega_2, alfa_2): 
    H_=kin_vel_H(zvednuti, omega_2, alfa_2)
    kv=kin_par(zvednuti, omega_2, alfa_2)
    fi_6=kv[3,0]
    omega_6=kv[3,1]
    alfa_6=kv[3,2]
    fi_T6=fi_6 #píst je rotačně symetrický, těžiště lěží tedy na spojnici bodů D a H
    L_T6=vzd_bodu(H,T_6)
    x_T6,y_T6=pol2cart(L_T6,fi_T6)
    vXT6=-omega_6*L_T6*np.sin(fi_T6)
    vYT6=omega_6*L_T6*np.cos(fi_T6)
    aXT6=-L_T6*np.cos(fi_T6)*omega_6**2-L_T6*np.sin(fi_T6)*alfa_6
    aYT6=-L_T6*np.sin(fi_T6)*omega_6**2+L_T6*np.cos(fi_T6)*alfa_6
    return np.array([[x_T6+H_[0,0],y_T6+H_[0,1]],[vXT6+H_[1,0],vYT6+H_[1,1]],[aXT6+H_[2,0],aYT6+H_[2,1]]])

def kin_vel_T39(zvednuti, omega_2, alfa_2):
    kv=kin_par(zvednuti, omega_2, alfa_2)
    fi_9=kv[6,0]
    omega_9=kv[6,1]
    alfa_9=kv[6,2]    
    D_=kin_vel_D(zvednuti, omega_2, alfa_2)
    fi_T39=fi_9+uhly(L_9, vzd_bodu(D,T_39), vzd_bodu(B, T_39))[2]
    L_T39=vzd_bodu(D,T_39)
    x_T39,y_T39=pol2cart(L_T39,fi_T39)    
    vXT39=-omega_9*L_T39*np.sin(fi_T39)
    vYT39=omega_9*L_T39*np.cos(fi_T39)
    aXT39=-L_T39*np.cos(fi_T39)*omega_9**2-L_T39*np.sin(fi_T39)*alfa_9
    aYT39=-L_T39*np.sin(fi_T39)*omega_9**2+L_T39*np.cos(fi_T39)*alfa_9
    return np.array([[x_T39+D_[0,0],y_T39+D_[0,1]],[vXT39+D_[1,0],vYT39+D_[1,1]],[aXT39+D_[2,0],aYT39+D_[2,1]]])

def kin_vel_J(zvednuti, omega_2, alfa_2):
    C=kin_vel_C(zvednuti, omega_2, alfa_2)
    kv=kin_par(zvednuti, omega_2, alfa_2)
    fi_12=kv[9,0] 
    omega_12=kv[9,1]
    alfa_12=kv[9,2] 
    x12,y12=prepocet_poloha(A, J, zvednuti)
    vX12=-omega_12*L_12*np.sin(fi_12)
    vY12=omega_12*L_12*np.cos(fi_12)
    aX12=-L_12*np.cos(fi_12)*omega_12**2-L_12*np.sin(fi_12)*alfa_12
    aY12=-L_12*np.sin(fi_12)*omega_12**2+L_12*np.cos(fi_12)*alfa_12
    return np.array([[x12,y12],[vX12+C[1,0],vY12+C[1,1]],[aX12+C[2,0],aY12+C[2,1]]])

def kin_vel_I(zvednuti, omega_2, alfa_2):
    J=kin_vel_J(zvednuti, omega_2, alfa_2)
    kv=kin_par(zvednuti, omega_2, alfa_2)
    fi_11=kv[8,0] 
    omega_11=kv[8,1]
    alfa_11=kv[8,2] 
    x11,y11=pol2cart(L_11,fi_11)
    vX11=-omega_11*L_11*np.sin(fi_11)
    vY11=omega_11*L_11*np.cos(fi_11)
    aX11=-L_11*np.cos(fi_11)*omega_11**2-L_11*np.sin(fi_11)*alfa_11
    aY11=-L_11*np.sin(fi_11)*omega_11**2+L_11*np.cos(fi_11)*alfa_11
    return np.array([[-x11+J[0,0],-y11+J[0,1]],[vX11+J[1,0],vY11+J[1,1]],[aX11+J[2,0],aY11+J[2,1]]])

def kin_vel_T11(zvednuti, omega_2, alfa_2): 
    kv=kin_par(zvednuti, omega_2, alfa_2)
    fi_11=kv[8,0]
    omega_11=kv[8,1]
    alfa_11=kv[8,2]
    I_=kin_vel_I(zvednuti, omega_2, alfa_2)
    fi_T11=fi_11+uhly(L_11, vzd_bodu(I,T_11), vzd_bodu(J, T_11))[2]
    L_T11=vzd_bodu(I,T_11)
    x_T11,y_T11=pol2cart(L_T11,fi_T11)
    vXT11=-omega_11*L_T11*np.sin(fi_T11)
    vYT11=omega_11*L_T11*np.cos(fi_T11)
    aXT11=-L_T11*np.cos(fi_T11)*omega_11**2-L_T11*np.sin(fi_T11)*alfa_11
    aYT11=-L_T11*np.sin(fi_T11)*omega_11**2+L_T11*np.cos(fi_T11)*alfa_11
    return np.array([[x_T11+I_[0,0],y_T11+I_[0,1]],[vXT11+I_[1,0],vYT11+I_[1,1]],[aXT11+I_[2,0],aYT11+I_[2,1]]])

def kin_vel_T10(zvednuti, omega_2, alfa_2): 
    kv=kin_par(zvednuti, omega_2, alfa_2)
    fi_10=kv[9,0]
    omega_10=kv[9,1]
    alfa_10=kv[9,2]
    H_=kin_vel_H(zvednuti, omega_2, alfa_2)
    fi_T10=fi_10 #fi_10-uhly(vzd_bodu(H,T_10), L_10, vzd_bodu(I, T_10))[2]
    L_T10=vzd_bodu(H, T_10)
    x_T10,y_T10=pol2cart(L_T10,fi_T10)
    vXT10=-omega_10*L_T10*np.sin(fi_T10)
    vYT10=omega_10*L_T10*np.cos(fi_T10)
    aXT10=-L_T10*np.cos(fi_T10)*omega_10**2-L_T10*np.sin(fi_T10)*alfa_10
    aYT10=-L_T10*np.sin(fi_T10)*omega_10**2+L_T10*np.cos(fi_T10)*alfa_10
    return np.array([[-x_T10+H_[0,0],-y_T10+H_[0,1]],[vXT10+H_[1,0],vYT10+H_[1,1]],[aXT10+H_[2,0],aYT10+H_[2,1]]])


def kin_vel_zatez(zvednuti, omega_2, alfa_2): 
    kv=kin_par(zvednuti, omega_2, alfa_2)
    fi_11=kv[8,0]
    omega_10=kv[8,1]
    alfa_10=kv[8,2]
    I_=kin_vel_I(zvednuti, omega_2, alfa_2)
    fi_T10=fi_11+uhly(L_11, vzd_bodu(I,Z), vzd_bodu(J, Z))[2]
    L_T10=vzd_bodu(I, Z)
    x_T10,y_T10=pol2cart(L_T10,fi_T10)
    vXT10=-omega_10*L_T10*np.sin(fi_T10)
    vYT10=omega_10*L_T10*np.cos(fi_T10)
    aXT10=-L_T10*np.cos(fi_T10)*omega_10**2-L_T10*np.sin(fi_T10)*alfa_10
    aYT10=-L_T10*np.sin(fi_T10)*omega_10**2+L_T10*np.cos(fi_T10)*alfa_10
    return np.array([[x_T10+I_[0,0],y_T10+I_[0,1]],[vXT10+I_[1,0],vYT10+I_[1,1]],[aXT10+I_[2,0],aYT10+I_[2,1]]])

I_zatez= m_zatez*(vzd_bodu(J,Z))**2

def sily(zvednuti, omega_2, alfa_2):
  

    rBx,rBy=vector(A,prepocet_poloha(A,B, zvednuti))
    rAx,rAy=vector(A,prepocet_poloha(A,C, zvednuti))    
    rFx,rFy=vector(A,prepocet_poloha(A,f, zvednuti))    
    rGx,rGy=vector(G,prepocet_poloha(G,E, zvednuti))
        
    rPx,rPy=vector(P,prepocet_poloha(A,f,zvednuti))
    beta=np.arctan(rPy/rPx)
       
    
    rHx,rHy=vector(kin_vel_D(zvednuti, omega_2, alfa_2)[0],kin_vel_H(zvednuti, omega_2, alfa_2)[0])
    rCx,rCy=vector(kin_vel_H(zvednuti, omega_2, alfa_2)[0],kin_vel_C(zvednuti, omega_2, alfa_2)[0])
    
    
    rDx,rDy=vector(prepocet_poloha(A,B, zvednuti),kin_vel_D(zvednuti, omega_2, alfa_2)[0]) #toto bylo blbe
    rEx,rEy=vector(prepocet_poloha(A,B, zvednuti),prepocet_poloha(G,E, zvednuti)) #taky
    
    rIx,rIy=vector(kin_vel_H(zvednuti, omega_2, alfa_2)[0],kin_vel_I(zvednuti, omega_2, alfa_2)[0])
    rJx,rJy=vector(kin_vel_J(zvednuti, omega_2, alfa_2)[0],kin_vel_I(zvednuti, omega_2, alfa_2)[0])  
    rJ1x,rJ1y=vector(A, kin_vel_J(zvednuti, omega_2, alfa_2)[0])
    
    rxT28,ryT28=vector(A,prepocet_poloha(A,T_28, zvednuti))
    rxT4,ryT4=vector(G,prepocet_poloha(G,T_4, zvednuti))
    rxT6,ryT6=vector(kin_vel_H(zvednuti, omega_2, alfa_2)[0],kin_vel_T6(zvednuti, omega_2, alfa_2)[0])
    rxT7,ryT7=vector( kin_vel_C(zvednuti, omega_2, alfa_2)[0],kin_vel_T7(zvednuti, omega_2, alfa_2)[0])
    rxT39,ryT39=vector(kin_vel_B(zvednuti, omega_2, alfa_2)[0],kin_vel_T39(zvednuti, omega_2, alfa_2)[0])
    rxT10,ryT10=vector(kin_vel_H(zvednuti, omega_2, alfa_2)[0],kin_vel_T10(zvednuti, omega_2, alfa_2)[0])
    
    rxTZ,ryTZ=vector(kin_vel_J(zvednuti, omega_2, alfa_2)[0],kin_vel_zatez(zvednuti, omega_2, alfa_2)[0]) 
    
    
    rxT11,ryT11=vector(kin_vel_J(zvednuti, omega_2, alfa_2)[0],kin_vel_T11(zvednuti, omega_2, alfa_2)[0])
    
    
    axT28,ayT28=kin_vel_T28(zvednuti, omega_2, alfa_2)[2]
    axT4,ayT4=kin_vel_T4(zvednuti, omega_2, alfa_2)[2]
    axT6,ayT6=kin_vel_T6(zvednuti, omega_2, alfa_2)[2]
    axT7,ayT7=kin_vel_T7(zvednuti, omega_2, alfa_2)[2]
    axT39,ayT39=kin_vel_T39(zvednuti, omega_2, alfa_2)[2]
    axT10,ayT10=kin_vel_T10(zvednuti, omega_2, alfa_2)[2]
    axT11,ayT11=kin_vel_T11(zvednuti, omega_2, alfa_2)[2]
    
    axZ,ayZ=kin_vel_zatez(zvednuti, omega_2, alfa_2)[2]
    
    alfa_4=kin_par(zvednuti, omega_2, alfa_2)[1][2]
    alfa_6=kin_par(zvednuti, omega_2, alfa_2)[3][2]
    alfa_7=kin_par(zvednuti, omega_2, alfa_2)[4][2]
    alfa_39=kin_par(zvednuti, omega_2, alfa_2)[0][2]
    alfa_10=kin_par(zvednuti, omega_2, alfa_2)[7][2]
    alfa_11=kin_par(zvednuti, omega_2, alfa_2)[8][2]
    
    
    I28=steiner(i28,T_28,m28)
    I4=steiner(i4,T_4,m4)
    I6=steiner(i6,T_6,m6)
    I7=steiner(i7,T_7,m7)
    I39=steiner(i39,T_39,m39)
    I10=steiner(i10,T_10,m10)
    I11=steiner(i11,T_11,m11)
    
    
    #[FAx, FAy, FBx, FBy, FCx, FCy, F, FDx, FDy, FEx, FEy, FGx, FGy, FHx6, FHy6, FHx7, FHy7, FHx10, FHy10, FIx, FIy, FJx, FJy]
    #  1    2    3    4    5    6   7   8    9   10   11   12   13    14    15    16    17    18     19     20   21   22   23 
    A__ = np.array(
    [[1, 0, 1, 0, 1, 0, np.cos(beta), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],#2+8+12
    [ 0, 1, 0, 1, 0, 1, np.sin(beta), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
    [0, 0, -rBy, rBx, -rAy, rAx, -rFy*np.cos(beta)+rFx*np.sin(beta), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -rJ1y, rJ1x],
    [0, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],#4
    [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
    [0, 0, 0, 0, 0, 0, 0, 0, 0, rGy, -rGx, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
    [0, 0, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0],#6
    [0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
    [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -rHy, rHx, 0, 0, 0, 0, 0, 0, 0, 0],
    [0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0], #7
    [0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0],
    [0, 0, 0, 0, rCy, -rCx, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
    [0, 0, -1, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],#3+9
    [0, 0, 0, -1, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
    [0, 0, 0, 0, 0, 0, 0, -rDy, rDx, -rEy, rEx, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
    [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, -1, 0, 0, 0],#10
    [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, -1, 0, 0],
    [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, rIy, -rIx, 0, 0],
    [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, -1, 0],#11
    [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, -1],
    [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -rJy, rJx, 0, 0],
    [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 1, 0, 1, 0, 0, 0, 0, 0],#13
    [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 1, 0, 1, 0, 0, 0, 0]])


    B__ = np.array([
        [m28*axT28],#2+8+12
        [m28*ayT28+FG28],
        [I28*alfa_2-m28*axT28*ryT28+m28*ayT28*rxT28+FG28*rxT28],
        [m4*axT4],#4
        [m4*ayT4+FG4],
        [I4*alfa_4-m4*axT4*ryT4+m4*ayT4*rxT4+FG4*rxT4],
        [m6*axT6],#6
        [m6*ayT6+FG6],
        [I6*alfa_6-m6*axT6*ryT6+m6*ayT6*rxT6+FG6*rxT6],
        [m7*axT7],#7
        [m7*ayT7+FG7],
        [I7*alfa_7-m7*axT7*ryT7-m7*ayT7*rxT7+FG7*rxT7],
        [m39*axT39],#39
        [m39*ayT39+FG39],
        [I39*alfa_39-m39*axT39*ryT39+m39*ayT39*rxT39+FG39*rxT39],
        [m10*axT10],#10
        [m10*ayT10+FG10],
        [I10*alfa_10-m10*axT10*ryT10+m10*ayT10*rxT10+FG10*rxT10],
        [m11*axT11+m_zatez*axZ],#11
        [m11*ayT11+FG11+FGzatez+m_zatez*ayZ],
        [I11*alfa_11-m11*axT11*ryT11+m11*ayT11*rxT11+FG11*rxT11+FGzatez*rxTZ+I_zatez*alfa_11-m_zatez*axZ*ryTZ+m_zatez*ayZ*rxTZ],
        [0],#13;
        [0],])
    
    
    # vyřešení soustavy rovnic
    
    sily = np.linalg.solve(A__, B__)
    sily=sily/2
    return sily

def kin_par(zvednuti, omega_2, alfa_2): 
    fi_2i=fi_2+deg2rad(zvednuti)
    fi_3i=fi_3
    fi_4i=fi_4+deg2rad(zvednuti)
    fi_5i=fi_5+deg2rad(zvednuti)
    fi_6i=fi_6+deg2rad(zvednuti)
    fi_7i=fi_7+deg2rad(zvednuti)
    fi_8i=fi_8+deg2rad(zvednuti)
    fi_9i=fi_9
    
    fi_10i=fi_10+deg2rad(zvednuti)
    fi_11i=fi_11+deg2rad(zvednuti)
    fi_12i=fi_12+deg2rad(zvednuti)      
     
    fi_12_=fi_12i
    
    root= fsolve(func,[fi_3i,fi_4i],args=(fi_2i), xtol=1e-5)
    fi_3i=root[0]
       
    
    root3= fsolve(func3,[fi_9i,fi_5i],args=(fi_3i), xtol=1e-5)
    fi_9i=root3[0]
    
    root2= fsolve(func2,[fi_6i,fi_7i],args=(fi_9i,fi_8i), xtol=1e-5)
    
    fi_7i=root2[1]
    root4= fsolve(func4,[fi_10i,fi_11i],args=(fi_7i,fi_12i), xtol=1e-5)
    
    fi_3i=root[0]
    fi_4i=root[1]
    fi_6i=root2[0]
    fi_7i=root2[1]
    fi_5i=root3[1]
    fi_9i=root3[0]
    
    fi_10i=root4[0]
    fi_11i=root4[1]
    
    # rychlosti smyčka A: L1+L2+L3+L4=0
    a = np.array([[-L_3*np.sin(fi_3i),-L_4*np.sin(fi_4i)], 
                  [L_3*np.cos(fi_3i),L_4*np.cos(fi_4i)]])
    
    b = np.array([L_2*np.sin(fi_2i)*omega_2, -L_2*np.cos(fi_2i)*omega_2])
    reseni_A = np.linalg.solve(a, b)
    omega_3=reseni_A[0]
    omega_4=reseni_A[1]
    
    # rychlosti smyčka B: L3+L5+L9=0
    a = np.array([[-L_5*np.sin(fi_5i),-L_9*np.sin(fi_9i)],  
                  [L_5*np.cos(fi_5i),L_9*np.cos(fi_9i)]])

    b = np.array([L_3*np.sin(fi_3i)*omega_3, -L_3*np.cos(fi_3i)*omega_3])
    reseni_B = np.linalg.solve(a, b)
    omega_5=reseni_B[0]
    omega_9=reseni_B[1]
    omega_8=omega_2
    omega_12=omega_2
    
    # rychlosti smyčka C: L6+L7+L8+L9=0
    a = np.array([[-L_6*np.sin(fi_6i),-L_7*np.sin(fi_7i)], 
                  [L_6*np.cos(fi_6i),L_7*np.cos(fi_7i)]])

    b = np.array([L_8*np.sin(fi_8i)*omega_8 + L_9*np.sin(fi_9i)*omega_9,-L_8*np.cos(fi_8i)*omega_8 - L_9*np.sin(fi_9i)*omega_9])
    reseni_C = np.linalg.solve(a, b)
    omega_6=reseni_C[0]
    omega_7=reseni_C[1]
    
    # rychlosti smyčka D: L7+L10+L11+L12=0
    a = np.array([[-L_10*np.sin(fi_10i),-L_11*np.sin(fi_11i)], 
                  [L_10*np.cos(fi_10i),L_11*np.cos(fi_11i)]])

    b = np.array([L_7*np.sin(fi_7i)*omega_7 + L_12*np.sin(fi_12i)*omega_12,-L_7*np.cos(fi_7i)*omega_7 - L_12*np.sin(fi_12i)*omega_12])
    reseni_D = np.linalg.solve(a, b)
    omega_10=reseni_D[0]
    omega_11=reseni_D[1]
    
    # zrychlení smyčka A: L1+L2+L3+L4=0
    a = np.array([[-L_3*np.sin(fi_3i),-L_4*np.sin(fi_4i)], 
                  [L_3*np.cos(fi_3i),L_4*np.cos(fi_4i)]])
    
    b = np.array([L_2*np.cos(fi_2i)*omega_2**2 + L_2*np.sin(fi_2i)*alfa_2 + L_3*np.cos(fi_3i)*omega_3**2 + L_4*np.cos(fi_4i)*omega_4**2,
                  L_2*np.sin(fi_2i)*omega_2**2 - L_2*np.cos(fi_2i)*alfa_2 + L_3*np.sin(fi_3i)*omega_3**2 + L_4*np.sin(fi_4i)*omega_4**2])
    reseni_A_z = np.linalg.solve(a, b)
    alfa_3=reseni_A_z[0]
    alfa_4=reseni_A_z[1]
    
    # zrychlení smyčka B: L3+L5+L9=0
    a = np.array([[-L_5*np.sin(fi_5i),-L_9*np.sin(fi_9i)],  
                  [L_5*np.cos(fi_5i),L_9*np.cos(fi_9i)]])

    b = np.array([L_3*np.cos(fi_3i)*omega_3**2 + L_3*np.sin(fi_3i)*alfa_3 + L_5*np.cos(fi_5i)*omega_5**2 + L_9*np.cos(fi_9i)*omega_9**2,
                  L_3*np.sin(fi_3i)*omega_3**2 - L_3*np.cos(fi_3i)*alfa_3 + L_5*np.sin(fi_5i)*omega_5**2 + L_9*np.sin(fi_9i)*omega_9**2])
    reseni_B_z = np.linalg.solve(a, b)
    alfa_5=reseni_B_z[0]
    alfa_9=reseni_B_z[1]
    alfa_8=alfa_2
    alfa_12=alfa_2
    # zrychlení smyčka C: L6+L7+L8+L9=0
    a = np.array([[-L_6*np.sin(fi_6i),-L_7*np.sin(fi_7i)], 
                  [L_6*np.cos(fi_6i),L_7*np.cos(fi_7i)]])

    b = np.array([L_6*np.cos(fi_6i)*omega_6**2 + L_7*np.cos(fi_7i)*omega_7**2 + L_8*np.cos(fi_8i)*omega_8**2 + L_8*np.sin(fi_8i)*alfa_8 +L_9*np.cos(fi_9i)*omega_9**2 + L_9*np.sin(fi_9i)*alfa_9,
                  L_6*np.sin(fi_6i)*omega_6**2 + L_7*np.sin(fi_7i)*omega_7**2 + L_8*np.sin(fi_8i)*omega_8**2 - L_8*np.cos(fi_8i)*alfa_8 +L_9*np.sin(fi_9i)*omega_9**2 - L_9*np.cos(fi_9i)*alfa_9])
    reseni_C_z = np.linalg.solve(a, b)
    alfa_6=reseni_C_z[0]
    alfa_7=reseni_C_z[1]
    
    # zrychlení smyčka D: L7+L10+L11+L12=0
    a = np.array([[-L_10*np.sin(fi_10i),-L_11*np.sin(fi_11i)],  
                  [L_10*np.cos(fi_10i),L_11*np.cos(fi_11i)]])

    b = np.array([L_10*np.cos(fi_10i)*omega_10**2 + L_11*np.cos(fi_11i)*omega_11**2 + L_7*np.cos(fi_7i)*omega_7**2 + L_7*np.sin(fi_7i)*alfa_7 +L_12*np.cos(fi_12i)*omega_12**2 + L_12*np.sin(fi_12i)*alfa_12,
                  L_10*np.sin(fi_10i)*omega_10**2 + L_11*np.sin(fi_11i)*omega_11**2 + L_7*np.sin(fi_7i)*omega_7**2 - L_7*np.cos(fi_7i)*alfa_7 +L_12*np.sin(fi_12i)*omega_12**2 - L_12*np.cos(fi_12i)*alfa_12])
    reseni_D_z = np.linalg.solve(a, b)
    alfa_10=reseni_D_z[0]
    alfa_11=reseni_D_z[1]
    
   
    fi_3_=root[0]
    fi_4_=root[1]
    fi_6_=root2[0]
    fi_7_=root2[1]
    fi_2_=fi_2i
    fi_9_=root3[0]
    fi_5_=root3[1]
    fi_8_=fi_8i
    fi_10_=root4[0]
    fi_11_=root4[1]
        
    #if zvednuti==0:       0                        1                         2                    3       
    return np.array([[fi_3_,omega_3,alfa_3],[fi_4_,omega_4,alfa_4],[fi_5_,omega_5,alfa_5],[fi_6_,omega_6,alfa_6], \
                     [fi_7_,omega_7,alfa_7],[fi_8_,omega_8,alfa_8],[fi_9_,omega_9,alfa_9],[fi_10_,omega_10,alfa_10],[fi_11_,omega_11,alfa_11],[fi_12_,omega_12,alfa_12]])
#                            4                      5                      6                            7                   8                               9                     10


FA_x=[]
sily_vypo=[]
kin_vypo=[]

link=np.linspace(0,res,res)

for i,j in enumerate(zvednuti_test):
    x=zvednuti_test[i] 
    sily_vypo_=sily(j, omega_2[i], alfa_2[i])
    sily_vypo.append(sily_vypo_)
    kin_vypo_=kin_par(j,omega_2[i], alfa_2[i])
    kin_vypo.append(kin_vypo_)
    
FA_x=np.zeros((res,1),dtype=float)
FA_y=np.zeros((res,1),dtype=float)
FB_x=np.zeros((res,1),dtype=float)
FB_y=np.zeros((res,1),dtype=float)
FC_x=np.zeros((res,1),dtype=float)
FC_y=np.zeros((res,1),dtype=float)
F=np.zeros((res,1),dtype=float)
FD_x=np.zeros((res,1),dtype=float)
FD_y=np.zeros((res,1),dtype=float)
FE_x=np.zeros((res,1),dtype=float)
FE_y=np.zeros((res,1),dtype=float)
FG_x=np.zeros((res,1),dtype=float)
FG_y=np.zeros((res,1),dtype=float)
FH_x=np.zeros((res,1),dtype=float)
FH_y=np.zeros((res,1),dtype=float)
zFI_4=np.zeros((res,1),dtype=float)
zalfa_4=np.zeros((res,1),dtype=float)
FH6_x=np.zeros((res,1),dtype=float)
FH7_x=np.zeros((res,1),dtype=float)
FH10_x=np.zeros((res,1),dtype=float)
FH6_y=np.zeros((res,1),dtype=float)
FH7_y=np.zeros((res,1),dtype=float)
FH10_y=np.zeros((res,1),dtype=float)
FI_x=np.zeros((res,1),dtype=float)
FI_y=np.zeros((res,1),dtype=float)
FJ_x=np.zeros((res,1),dtype=float)
FJ_y=np.zeros((res,1),dtype=float)

FA=np.zeros((res,1),dtype=float)
FB=np.zeros((res,1),dtype=float)
FC=np.zeros((res,1),dtype=float)
FD=np.zeros((res,1),dtype=float)
FE=np.zeros((res,1),dtype=float)
FG=np.zeros((res,1),dtype=float)
FH=np.zeros((res,1),dtype=float)
FI=np.zeros((res,1),dtype=float)
FJ=np.zeros((res,1),dtype=float)



for i in range(res):
    FA_x[i]=sily_vypo[i][0]
    FA_y[i]=sily_vypo[i][1]
    FB_x[i]=sily_vypo[i][2]
    FB_y[i]=sily_vypo[i][3]
    FC_x[i]=sily_vypo[i][4]
    FC_y[i]=sily_vypo[i][5]
    F[i]=sily_vypo[i][6]
    FD_x[i]=sily_vypo[i][7]
    FD_y[i]=sily_vypo[i][8]
    FE_x[i]=sily_vypo[i][9]
    FE_y[i]=sily_vypo[i][10]
    FG_x[i]=sily_vypo[i][11]
    FG_y[i]=sily_vypo[i][12]
    FH6_x[i]=sily_vypo[i][13]
    FH6_y[i]=sily_vypo[i][14]
    FI_x[i]=sily_vypo[i][19]
    FI_y[i]=sily_vypo[i][20]
    FJ_x[i]=sily_vypo[i][21]
    FJ_y[i]=sily_vypo[i][22]
    FH7_x[i]=sily_vypo[i][15]
    FH7_y[i]=sily_vypo[i][16]
    FH10_x[i]=sily_vypo[i][17]
    FH10_y[i]=sily_vypo[i][18]
    
def get_F_abs(F_x,F_y):
    F=np.sqrt(F_x**2+F_y**2)
    return F

FA=get_F_abs(FA_x,FA_y)
FB=get_F_abs(FB_x,FB_y)
FC=get_F_abs(FC_x,FC_y)
FD=get_F_abs(FD_x,FD_y)
FE=get_F_abs(FE_x,FE_y)
FI=get_F_abs(FI_x,FI_y)   
FJ=get_F_abs(FJ_x,FJ_y)
FG=get_F_abs(FG_x,FG_y)

FH6=get_F_abs(FH6_x,FH6_y)
FH7=get_F_abs(FH7_x,FH7_y)
FH10=get_F_abs(FH10_x,FH10_y)

plot_graphs=1

if plot_graphs==1:
    pic_out='no'
    
    font_size = 12 #10
    ntch_ls_wdth=1.2
    expand=1.2 
    if pic_out=='yes':
        mpl.rcdefaults()
        style = {
                'figure.figsize' : [2.0*4.82, 1.8*2.98], # gold cut [2*4.82, 3*2.98]         
                'font.size' : font_size,
                'legend.fontsize' : font_size,
                'legend.numpoints' : 1,
                'axes.linewidth' : 0.8, 
                'patch.linewidth' : 0.8,
                'lines.linewidth' : 0.8,
                "text.usetex": True,                
                "font.family": "sans-serif",
                "text.latex.preamble" : r"\usepackage{cmbright}"
                }
        mpl.rcParams.update(style)
    elif pic_out=='no':
        mpl.rcdefaults()
        style = {
                'figure.figsize' : [1.8*4.82, 2.8*2.98], # gold cut [2*4.82, 3*2.98]         
                'font.size' : font_size,
                'legend.fontsize' : font_size,
                'legend.numpoints' : 1,
                'axes.linewidth' : 0.8, 
                'patch.linewidth' : 0.8,
                'lines.linewidth' : 0.8,
                "text.usetex": False,                
                "font.family": "sans-serif"
                }
        mpl.rcParams.update(style) 
    fileloc='Results/'
    fig, ax = plt.subplots(2,1)
    #ax1 = ax[0].twinx()
    p1,=ax[0].plot(cas_test1, FA_x*1e-3, 'b', label='$F_{Ax}$')
    p2,=ax[0].plot(cas_test1, FA_y*1e-3, 'r', label='$F_{Ay}$')
    p3,=ax[0].plot(cas_test1, FA*1e-3, 'g', label='$F_{A}$')
    ax[0].set_ylabel('$F$ [$\mathrm{kN}$]')
    ax[0].set_xlabel('$t$ [s]')
    ax[0].grid(True)
    ax[0].margins(x=0)
    lines = [p1, p2, p3]
    lgd1=ax[0].legend(lines, [li.get_label() for li in lines],loc='lower center',ncol=3,handlelength=2.5,borderaxespad =0.0, bbox_to_anchor=(0.00, 1.09,1.0,0.0),mode='expand')
    
    #ax1 = ax[0].twinx()
    p1,=ax[1].plot(cas_test1, FG_x*1e-3, 'b', label='$F_{Gx}$')
    p2,=ax[1].plot(cas_test1, FG_y*1e-3, 'r', label='$F_{Gy}$')
    p3,=ax[1].plot(cas_test1, FG*1e-3, 'g', label='$F_{G}$')
    ax[1].set_ylabel('$F$ [$\mathrm{kN}$]')
    ax[1].set_xlabel('$t$ [s]')
    ax[1].grid(True)
    ax[1].margins(x=0)
    lines = [p1, p2, p3]
    lgd1=ax[1].legend(lines, [li.get_label() for li in lines],loc='lower center',ncol=3,handlelength=2.5,borderaxespad =0.0, bbox_to_anchor=(0.00, 1.09,1.0,0.0),mode='expand')
    plt.tight_layout()
    plt.show()
    
    fig, ax = plt.subplots(2,1)
    #ax1 = ax[0].twinx()
    p1,=ax[0].plot(cas_test1, FE_x*1e-3, 'b', label='$F_{Ex}$')
    p2,=ax[0].plot(cas_test1, FE_y*1e-3, 'r', label='$F_{Ey}$')
    p3,=ax[0].plot(cas_test1, FE*1e-3, 'g', label='$F_{E}$')
    ax[0].set_ylabel('$F$ [$\mathrm{kN}$]')
    ax[0].set_xlabel('$t$ [s]')
    ax[0].grid(True)
    ax[0].margins(x=0)
    lines = [p1, p2, p3]
    lgd1=ax[0].legend(lines, [li.get_label() for li in lines],loc='lower center',ncol=3,handlelength=2.5,borderaxespad =0.0, bbox_to_anchor=(0.00, 1.09,1.0,0.0),mode='expand')
    
    #ax1 = ax[0].twinx()
    p1,=ax[1].plot(cas_test1, FD_x*1e-3, 'b', label='$F_{Dx}$')
    p2,=ax[1].plot(cas_test1, FD_y*1e-3, 'r', label='$F_{Dy}$')
    p3,=ax[1].plot(cas_test1, FD*1e-3, 'g', label='$F_{D}$')
    ax[1].set_ylabel('$F$ [$\mathrm{kN}$]')
    ax[1].set_xlabel('$t$ [s]')
    ax[1].grid(True)
    ax[1].margins(x=0)
    lines = [p1, p2, p3]
    lgd1=ax[1].legend(lines, [li.get_label() for li in lines],loc='lower center',ncol=3,handlelength=2.5,borderaxespad =0.0, bbox_to_anchor=(0.00, 1.09,1.0,0.0),mode='expand')
    plt.tight_layout()
    plt.show()
    
    fig, ax = plt.subplots(2,1)
    #ax1 = ax[0].twinx()
    p1,=ax[0].plot(cas_test1, F*1e-3, 'b', label='$F$')
    
    ax[0].set_ylabel('$F$ [$\mathrm{kN}$]')
    ax[0].set_xlabel('$t$ [s]')
    ax[0].grid(True)
    ax[0].margins(x=0)
    lines = [p1]
    lgd1=ax[0].legend(lines, [li.get_label() for li in lines],loc='lower center',ncol=3,handlelength=2.5,borderaxespad =0.0, bbox_to_anchor=(0.00, 1.09,1.0,0.0),mode='expand')
    
    #ax1 = ax[0].twinx()
    p1,=ax[1].plot(cas_test1, FB_x*1e-3, 'b', label='$F_{Bx}$')
    p2,=ax[1].plot(cas_test1, FB_y*1e-3, 'r', label='$F_{By}$')
    p3,=ax[1].plot(cas_test1, FB*1e-3, 'g', label='$F_{B}$')
    ax[1].set_ylabel('$F$ [$\mathrm{kN}$]')
    ax[1].set_xlabel('$t$ [s]')
    ax[1].grid(True)
    ax[1].margins(x=0)
    lines = [p1, p2, p3]
    lgd1=ax[1].legend(lines, [li.get_label() for li in lines],loc='lower center',ncol=3,handlelength=2.5,borderaxespad =0.0, bbox_to_anchor=(0.00, 1.09,1.0,0.0),mode='expand')
    plt.tight_layout()
    plt.show()
    
    fig, ax = plt.subplots(2,1)
    #ax1 = ax[0].twinx()
    p1,=ax[0].plot(cas_test1, FC_x*1e-3, 'b', label='$F_{Cx}$')
    p2,=ax[0].plot(cas_test1, FC_y*1e-3, 'r', label='$F_{Cy}$')
    p3,=ax[0].plot(cas_test1, FC*1e-3, 'g', label='$F_{C}$')
    ax[0].set_ylabel('$F$ [$\mathrm{kN}$]')
    ax[0].set_xlabel('$t$ [s]')
    ax[0].grid(True)
    ax[0].margins(x=0)
    lines = [p1, p2, p3]
    lgd1=ax[0].legend(lines, [li.get_label() for li in lines],loc='lower center',ncol=3,handlelength=2.5,borderaxespad =0.0, bbox_to_anchor=(0.00, 1.09,1.0,0.0),mode='expand')
    
    #ax1 = ax[0].twinx()
    p1,=ax[1].plot(cas_test1, FH6_x*1e-3, 'b', label='$F_{H6x}$')
    p2,=ax[1].plot(cas_test1, FH6_y*1e-3, 'r', label='$F_{H6y}$')
    p3,=ax[1].plot(cas_test1, FH6*1e-3, 'g', label='$F_{H6}$')
    ax[1].set_ylabel('$F$ [$\mathrm{kN}$]')
    ax[1].set_xlabel('$t$ [s]')
    ax[1].grid(True)
    ax[1].margins(x=0)
    lines = [p1, p2, p3]
    lgd1=ax[1].legend(lines, [li.get_label() for li in lines],loc='lower center',ncol=3,handlelength=2.5,borderaxespad =0.0, bbox_to_anchor=(0.00, 1.09,1.0,0.0),mode='expand')
    plt.tight_layout()
    plt.show()
    
    fig, ax = plt.subplots(2,1)
    #ax1 = ax[0].twinx()
    p1,=ax[0].plot(cas_test1, FI_x*1e-3, 'b', label='$F_{Ix}$')
    p2,=ax[0].plot(cas_test1, FI_y*1e-3, 'r', label='$F_{Iy}$')
    p3,=ax[0].plot(cas_test1, FI*1e-3, 'g', label='$F_{I}$')
    ax[0].set_ylabel('$F$ [$\mathrm{kN}$]')
    ax[0].set_xlabel('$t$ [s]')
    ax[0].grid(True)
    ax[0].margins(x=0)
    lines = [p1, p2, p3]
    lgd1=ax[0].legend(lines, [li.get_label() for li in lines],loc='lower center',ncol=3,handlelength=2.5,borderaxespad =0.0, bbox_to_anchor=(0.00, 1.09,1.0,0.0),mode='expand')
    
    #ax1 = ax[0].twinx()
    p1,=ax[1].plot(cas_test1, FJ_x*1e-3, 'b', label='$F_{Jx}$')
    p2,=ax[1].plot(cas_test1, FJ_y*1e-3, 'r', label='$F_{Jy}$')
    p3,=ax[1].plot(cas_test1, FJ*1e-3, 'g', label='$F_{J}$')
    ax[1].set_ylabel('$F$ [$\mathrm{kN}$]')
    ax[1].set_xlabel('$t$ [s]')
    ax[1].grid(True)
    ax[1].margins(x=0)
    lines = [p1, p2, p3]
    lgd1=ax[1].legend(lines, [li.get_label() for li in lines],loc='lower center',ncol=3,handlelength=2.5,borderaxespad =0.0, bbox_to_anchor=(0.00, 1.09,1.0,0.0),mode='expand')
    plt.tight_layout()
    plt.show()
    
    fig, ax = plt.subplots(2,1)
    #ax1 = ax[0].twinx()
    p1,=ax[0].plot(cas_test1, FH7_x*1e-3, 'b', label='$F_{H7x}$')
    p2,=ax[0].plot(cas_test1, FH7_y*1e-3, 'r', label='$F_{H7y}$')
    p3,=ax[0].plot(cas_test1, FH7*1e-3, 'g', label='$F_{H7}$')
    ax[0].set_ylabel('$F$ [$\mathrm{kN}$]')
    ax[0].set_xlabel('$t$ [s]')
    ax[0].grid(True)
    ax[0].margins(x=0)
    lines = [p1, p2, p3]
    lgd1=ax[0].legend(lines, [li.get_label() for li in lines],loc='lower center',ncol=3,handlelength=2.5,borderaxespad =0.0, bbox_to_anchor=(0.00, 1.09,1.0,0.0),mode='expand')
    
    #ax1 = ax[0].twinx()
    p1,=ax[1].plot(cas_test1, FH10_x*1e-3, 'b', label='$F_{H10x}$')
    p2,=ax[1].plot(cas_test1, FH10_y*1e-3, 'r', label='$F_{H10y}$')
    p3,=ax[1].plot(cas_test1, FH10*1e-3, 'g', label='$F_{H10}$')
    ax[1].set_ylabel('$F$ [$\mathrm{kN}$]')
    ax[1].set_xlabel('$t$ [s]')
    ax[1].grid(True)
    ax[1].margins(x=0)
    lines = [p1, p2, p3]
    lgd1=ax[1].legend(lines, [li.get_label() for li in lines],loc='lower center',ncol=3,handlelength=2.5,borderaxespad =0.0, bbox_to_anchor=(0.00, 1.09,1.0,0.0),mode='expand')
    plt.tight_layout()
    plt.show()




def extract_acc(zvednuti,f,omega_2,alfa_2):
    x_,y_=f(zvednuti, omega_2, alfa_2)[0]
    ax_,ay_=f(zvednuti, omega_2, alfa_2)[2]
    return x_,y_,ax_,ay_

def get_arrow_forces(zvednuti,omega_2,alfa_2,ix,iy):
    sily_=sily(zvednuti, omega_2, alfa_2)
    
    #[FAx, FAy, FBx, FBy, FCx, FCy, F, FDx, FDy, FEx, FEy, FGx, FGy, FHx, FHy]
    #  1    2    3    4    5    6   7   8    9   10   11   12   13   14   15    
    
    
    a_N_=sily_[6]
    factor=2e-5
    #beta=np.arctan2(ayT,axT)
    rPx,rPy=vector(P,prepocet_poloha(A,f,zvednuti))
    beta=np.arctan(rPy/rPx)
    x_2Ne,y_2Ne=pol2cart(a_N_*factor,beta)
    return rPx+P[0],rPy+P[1],x_2Ne[0],y_2Ne[0]

def get_F_ANSYS(zvednuti,omega_2,alfa_2,ix,iy):
    sily_=sily(zvednuti, omega_2, alfa_2)
    
    #[FAx, FAy, FBx, FBy, FCx, FCy, F, FDx, FDy, FEx, FEy, FGx, FGy, FHx, FHy]
    #  1    2    3    4    5    6   7   8    9   10   11   12   13   14   15    
    
    #a_N_=np.sqrt(axT**2+ayT**2)
    a_N_=sily_[6]
    factor=1
    #beta=np.arctan2(ayT,axT)
    rPx,rPy=vector(P,prepocet_poloha(A,f,zvednuti))
    beta=np.arctan(rPy/rPx)
    x_2Ne,y_2Ne=pol2cart(a_N_*factor,beta)
    return -x_2Ne[0],-y_2Ne[0]    
F_x=[]
F_y=[]
for i,j in enumerate(zvednuti_test):  
    F_x_,F_y_=get_F_ANSYS(j,omega_2[i],alfa_2[i],0,0)
    F_x.append(F_x_)
    F_y.append(F_y_)
    

def get_arrow(zvednuti,f,omega_2,alfa_2,bod_otaceni,bod):    
    x_,y_,axT,ayT=extract_acc(zvednuti,f,omega_2,alfa_2)
    a_N_=np.sqrt(axT**2+ayT**2)
    factor=2
    beta=np.arctan2(ayT,axT)
    x_2Ne,y_2Ne=pol2cart(a_N_*factor,beta) 
    #x_,y_=prepocet_poloha(bod_otaceni,bod, zvednuti)
    return x_,y_,x_2Ne,y_2Ne
    
    
i=0
fig, ax = plt.subplots(1,1)

zve=zvednuti_test[i]
omega_2_=omega_2[i]
alfa_2_=alfa_2[i]
X_A,Y_A=A
X_G,Y_G=G
X_P,Y_P=P
X_f,Y_f=prepocet_poloha(A,f,zve)
X_Z,Y_Z=kin_vel_zatez(zve,omega_2_,alfa_2_)[0]#polo_Z(zve, omega_2_, alfa_2_)
X_B,Y_B=kin_vel_B(zve,omega_2_,alfa_2_)[0]
X_E,Y_E=kin_vel_E(zve,omega_2_,alfa_2_)[0]
X_C,Y_C=kin_vel_C(zve,omega_2_,alfa_2_)[0]
X_H,Y_H=kin_vel_H(zve,omega_2_,alfa_2_)[0]
X_D,Y_D=kin_vel_D(zve,omega_2_,alfa_2_)[0]
X_I,Y_I=kin_vel_I(zve,omega_2_,alfa_2_)[0]
X_J,Y_J=kin_vel_J(zve,omega_2_,alfa_2_)[0]

X_T28,Y_T28=kin_vel_T28(zve,omega_2_,alfa_2_)[0]
X_T39,Y_T39=kin_vel_T39(zve,omega_2_,alfa_2_)[0]
X_T4,Y_T4=kin_vel_T4(zve,omega_2_,alfa_2_)[0]
X_T6,Y_T6=kin_vel_T6(zve,omega_2_,alfa_2_)[0]
X_T7,Y_T7=kin_vel_T7(zve,omega_2_,alfa_2_)[0]
X_T10,Y_T10=kin_vel_T10(zve,omega_2_,alfa_2_)[0]
X_T11,Y_T11=kin_vel_T11(zve,omega_2_,alfa_2_)[0]


ax.clear()
#
ax.plot(0,0, 'o', color='r', markersize=5) 

ax.plot([0,X_A],[0,Y_A], color='b') 
ax.plot(X_Z,Y_Z, 'o', color='b', markersize=5) 
ax.plot(Z[0],Z[1], 'o', color='magenta', markersize=5) 
ax.plot([X_A,X_B],[Y_A,Y_B], color='b') 
ax.plot([X_B,X_E],[Y_B,Y_E], color='b') 
ax.plot([X_E,X_G],[Y_E,Y_G], color='b')


ax.plot([X_B,X_D],[Y_B,Y_D], color='g')
ax.plot([X_B,X_C],[Y_B,Y_C], color='b')
ax.plot([X_C,X_H],[Y_C,Y_H], color='g') 
ax.plot([X_H,X_D],[Y_H,Y_D], color='r')
ax.plot([X_P,X_f],[Y_P,Y_f], color='y')

ax.plot([X_H,X_I],[Y_H,Y_I], color='y')
ax.plot([X_I,X_J],[Y_I,Y_J], color='r')
ax.plot([X_C,X_J],[Y_C,Y_J], color='y')

ax.plot(X_T28,Y_T28,'o', color='k')
ax.plot(X_T39,Y_T39,'o', color='b')
ax.plot(X_T4,Y_T4,'o', color='r')
ax.plot(X_T7,Y_T7,'o', color='g')
ax.plot(X_T6,Y_T6,'o', color='y')
ax.plot(X_T10,Y_T10,'o', color='y')
ax.plot(X_T11,Y_T11,'o', color='r')

body=[T_28,T_39,T_4,T_7,T_6, T_10,T_11]
bod_otaceni_=[A,kin_vel_D(zve, omega_2[i], alfa_2[i])[0],G,kin_vel_C(zve, omega_2[i], alfa_2[i])[0],kin_vel_H(zve, omega_2[i], alfa_2[i])[0], kin_vel_H(zve, omega_2[i], alfa_2[i])[0], kin_vel_I(zve, omega_2[i], alfa_2[i])[0] ]
funkce=[kin_vel_T28,kin_vel_T39,kin_vel_T4,kin_vel_T7,kin_vel_T6,kin_vel_T10,kin_vel_T11]
for i,bod in enumerate(body):
    x_,y_,x_2Ne,y_2Ne=get_arrow(zve,funkce[i],omega_2[i],alfa_2[i],bod_otaceni_[i],bod)
    ax.arrow(x_,y_,x_2Ne,y_2Ne,width=0.01,head_width=0.1,ec='g',fc='g')
    

x_,y_,x_2Ne,y_2Ne=get_arrow_forces(zve,omega_2[i],alfa_2[i],0,1)
ax.arrow(x_,y_,x_2Ne,y_2Ne,width=0.01,head_width=0.1,ec='r',fc='r')


ax.set_xlim([-0.5,4.5])
ax.set_ylim([-2,2])
ax.axis('equal')
plt.show()    



plot2=1
if plot2==1:

    pic_out='no'
    
    font_size = 12 #10
    ntch_ls_wdth=1.2
    expand=1.2 
    if pic_out=='yes':
        mpl.rcdefaults()
        style = {
                'figure.figsize' : [2.0*4.82, 1.8*2.98], # gold cut [2*4.82, 3*2.98]         
                'font.size' : font_size,
                'legend.fontsize' : font_size,
                'legend.numpoints' : 1,
                'axes.linewidth' : 0.8, 
                'patch.linewidth' : 0.8,
                'lines.linewidth' : 0.8,
                "text.usetex": True,                
                "font.family": "sans-serif",
                "text.latex.preamble" : r"\usepackage{cmbright}"
                }
        mpl.rcParams.update(style)
    elif pic_out=='no':
        mpl.rcdefaults()
        style = {
                'figure.figsize' : [1.8*4.82, 2.8*2.98], # gold cut [2*4.82, 3*2.98]         
                'font.size' : font_size,
                'legend.fontsize' : font_size,
                'legend.numpoints' : 1,
                'axes.linewidth' : 0.8, 
                'patch.linewidth' : 0.8,
                'lines.linewidth' : 0.8,
                "text.usetex": False,                
                "font.family": "sans-serif"
                }
        mpl.rcParams.update(style) 
    fileloc='Results/'
    fig, ax = plt.subplots(3,1)
    #ax1 = ax[0].twinx()
    #ax2 = ax[1].twinx()
    ax3 = ax[2].twinx()
    
    p1,=ax[0].plot(cas_test1, [kin_vypo[i][0,0] for i in range(np.size(zvednuti_test))], 'b', label=r'$\varphi_{3}$')
    p2,=ax[0].plot(cas_test1, [kin_vypo[i][1,0] for i in range(np.size(zvednuti_test))], 'r', label=r'$\varphi_{4}$')
    p3,=ax[0].plot(cas_test1, [kin_vypo[i][2,0] for i in range(np.size(zvednuti_test))], 'g', label=r'$\varphi_{5}$')
    p4,=ax[0].plot(cas_test1, [kin_vypo[i][3,0] for i in range(np.size(zvednuti_test))], 'k', label=r'$\varphi_{6}$')
    p5,=ax[0].plot(cas_test1, [kin_vypo[i][4,0] for i in range(np.size(zvednuti_test))], 'gray', label=r'$\varphi_{7}$')
    p6,=ax[0].plot(cas_test1, [kin_vypo[i][5,0] for i in range(np.size(zvednuti_test))], 'magenta', label=r'$\varphi_{8}$')
    p7,=ax[0].plot(cas_test1, [kin_vypo[i][6,0] for i in range(np.size(zvednuti_test))], 'cyan', label=r'$\varphi_{9}$')
    
    ax[0].set_ylabel(r'$\varphi$ [$\mathrm{rad}$]')
    
    ax[0].margins(x=0)
    lines = [p1, p2, p3, p4, p5, p6, p7]
    lgd1=ax[0].legend(lines, [li.get_label() for li in lines],loc='lower center',ncol=9,handlelength=2.5,borderaxespad =0.0, bbox_to_anchor=(0.00, 1.09,1.0,0.0),mode='expand')
    
    p1,=ax[1].plot(cas_test1, [kin_vypo[i][0,1] for i in range(np.size(zvednuti_test))], 'b', label='$\omega_{3}$')
    p2,=ax[1].plot(cas_test1, [kin_vypo[i][1,1] for i in range(np.size(zvednuti_test))], 'r', label='$\omega_{4}$')
    p3,=ax[1].plot(cas_test1, [kin_vypo[i][2,1] for i in range(np.size(zvednuti_test))], 'g', label='$\omega_{5}$')
    p4,=ax[1].plot(cas_test1, [kin_vypo[i][3,1] for i in range(np.size(zvednuti_test))], 'k', label='$\omega_{6}$')
    p5,=ax[1].plot(cas_test1, [kin_vypo[i][4,1] for i in range(np.size(zvednuti_test))], 'gray', label='$\omega_{7}$')
    p6,=ax[1].plot(cas_test1, [kin_vypo[i][5,1] for i in range(np.size(zvednuti_test))], 'magenta', label='$\omega_{8}$')
    p7,=ax[1].plot(cas_test1, [kin_vypo[i][6,1] for i in range(np.size(zvednuti_test))], 'cyan', label='$\omega_{9}$')
    
    ax[1].set_ylabel('$\omega$ [$\mathrm{rad}$]')
    
    ax[1].margins(x=0)
    lines = [p1, p2, p3, p4, p5, p6, p7]
    lgd1=ax[1].legend(lines, [li.get_label() for li in lines],loc='lower center',ncol=9,handlelength=2.5,borderaxespad =0.0, bbox_to_anchor=(0.00, 1.09,1.0,0.0),mode='expand')
    
    p1,=ax[2].plot(cas_test1, [kin_vypo[i][0,2] for i in range(np.size(zvednuti_test))], 'b', label=r'$\alpha_{3}$')
    p2,=ax[2].plot(cas_test1, [kin_vypo[i][1,2] for i in range(np.size(zvednuti_test))], 'r', label=r'$\alpha_{4}$')
    p3,=ax[2].plot(cas_test1, [kin_vypo[i][2,2] for i in range(np.size(zvednuti_test))], 'g', label=r'$\alpha_{5}$')
    p4,=ax[2].plot(cas_test1, [kin_vypo[i][3,2] for i in range(np.size(zvednuti_test))], 'k', label=r'$\alpha_{6}$')
    p5,=ax[2].plot(cas_test1, [kin_vypo[i][4,2] for i in range(np.size(zvednuti_test))], 'gray', label=r'$\alpha_{7}$')
    p6,=ax[2].plot(cas_test1, [kin_vypo[i][5,2] for i in range(np.size(zvednuti_test))], 'magenta', label=r'$\alpha_{8}$')
    p7,=ax[2].plot(cas_test1, [kin_vypo[i][6,2] for i in range(np.size(zvednuti_test))], 'cyan', label=r'$\alpha_{9}$')
    
    ax[2].set_ylabel('$\omega$ [$\mathrm{rad}$]')
    
    ax[2].margins(x=0)
    lines = [p1, p2, p3, p4, p5, p6, p7]
    lgd1=ax[2].legend(lines, [li.get_label() for li in lines],loc='lower center',ncol=9,handlelength=2.5,borderaxespad =0.0, bbox_to_anchor=(0.00, 1.09,1.0,0.0),mode='expand')
    
    
    
    plt.tight_layout()
    plt.xlabel('theta')
    ax[0].grid(True)
    ax[1].grid(True)
    ax[2].grid(True)
    if pic_out=='yes':
            plt.savefig(fileloc+'Sily.jpg', dpi=600,transparent=True,bbox_extra_artists=(lgd1,),bbox_inches = 'tight')
    
    plt.show()
    
    
    pic_out='no'
    
    font_size = 12 #10
    ntch_ls_wdth=1.2
    expand=1.2 
    if pic_out=='yes':
        mpl.rcdefaults()
        style = {
                'figure.figsize' : [2.0*4.82, 1.8*2.98], # gold cut [2*4.82, 3*2.98]         
                'font.size' : font_size,
                'legend.fontsize' : font_size,
                'legend.numpoints' : 1,
                'axes.linewidth' : 0.8, 
                'patch.linewidth' : 0.8,
                'lines.linewidth' : 0.8,
                "text.usetex": True,                
                "font.family": "sans-serif",
                "text.latex.preamble" : r"\usepackage{cmbright}"
                }
        mpl.rcParams.update(style)
    elif pic_out=='no':
        mpl.rcdefaults()
        style = {
                'figure.figsize' : [1.8*4.82, 2.8*2.98], # gold cut [2*4.82, 3*2.98]         
                'font.size' : font_size,
                'legend.fontsize' : font_size,
                'legend.numpoints' : 1,
                'axes.linewidth' : 0.8, 
                'patch.linewidth' : 0.8,
                'lines.linewidth' : 0.8,
                "text.usetex": False,                
                "font.family": "sans-serif"
                }
        mpl.rcParams.update(style) 
    fileloc='Results/'
    for j in (2,3,4,5,6,7,8,9,10,11,12):
        
    
        fig, ax = plt.subplots(3,1)
        
        
        p1,=ax[0].plot(cas_test1, [kin_vypo[i][j-3,0] for i in range(np.size(zvednuti_test))], 'b', label=r'$\varphi_{'+str(j)+'}$')
        p2,=ax[1].plot(cas_test1, [kin_vypo[i][j-3,1] for i in range(np.size(zvednuti_test))], 'r', label=r'$\omega_{'+str(j)+'}$')
        p3,=ax[2].plot(cas_test1, [kin_vypo[i][j-3,2] for i in range(np.size(zvednuti_test))], 'g', label=r'$\alpha_{'+str(j)+'}$')
        
        
        ax[0].set_ylabel(r'$\varphi$ [$\mathrm{rad}$]')
        ax[1].set_ylabel(r'$\omega$ [$\mathrm{rad/s}$]')
        ax[2].set_ylabel(r'$\alpha$ [$\mathrm{rad/s^{-2}}$]')
        
        ax[0].margins(x=0)
        ax[1].margins(x=0)
        ax[2].margins(x=0)
        lines = [p1, p2, p3]
        lgd1=ax[0].legend(lines, [li.get_label() for li in lines],loc='lower center',ncol=9,handlelength=2.5,borderaxespad =0.0, bbox_to_anchor=(0.00, 1.09,1.0,0.0),mode='expand')
        
        
        
        plt.tight_layout()
        plt.xlabel('$t$ [s]')
        ax[0].grid(True)
        ax[1].grid(True)
        ax[2].grid(True)
        
        if pic_out=='yes':
                plt.savefig(fileloc+'Sily.jpg', dpi=600,transparent=True,bbox_extra_artists=(lgd1,),bbox_inches = 'tight')
        
        plt.show()

   
    
ANS_FAx=vyber_ANSYS(-FA_x)
ANS_cas=vyber_ANSYS(cas_test1)
ANS_FAy=vyber_ANSYS(-FA_y)

ANS_FGx=vyber_ANSYS(-FG_x)
ANS_FGy=vyber_ANSYS(-FG_y)

ANS_F_x=vyber_ANSYS(F_x)
ANS_F_y=vyber_ANSYS(F_y)   
    
    
    
    
    
