'''
  This file contains a representation of the solution of the robotic arm DAE, given by DeLuca, Isidori, cf.
  Singularities of the Robotic Arm DAE by Diana Este'vez Schwarz, Rene' Lamour, Roswitha Maerz
  in DAE Forum 2019
'''
import algopy
from numpy import sin, cos, exp,zeros as npzeros, array
from algopy import zeros

def RoboticArmSolutionDeLu87(t):
    """
    input t - timepoint, where the solution is computed
    output x - solution vector x(t)
           xabl - derivative x'(t) of the components x1,...,x6. x7'(t) and x8'(t) is set to zero.
    """ 
    A2=2.
    A3=1.
    A4=2.
    A5=2.
    NT=2.
    K=4.
    JR1=1.
    expt=exp(t)
    x1=1. - expt
    x3=expt-t
    cosx3=cos(x3)
    cos2x3=cos(2*(x3))
    cos3x3=cos(3*(x3))
   
    
    sinx3=sin(x3)
    sin2x3=sin(2*(x3))
    
    x4=- expt
    x4prime=- expt
    x4second=- expt
    x4third=- expt
    x6=expt-1.
    x6prime=expt
    x6second=expt
    x6third=expt
    
    x2=        (NT*(K*(A2*A5 - 4*A2*A3*cosx3 - A3**2*cosx3**2)*x3 + A3*(A2*A5 - A3**2*cosx3**2)*sinx3*x4**2\
        + A3*(A4 - A3*cosx3**2)*((A2 + A3*cosx3)*x4prime + A2*x6prime)))/(K*(A2*A5 - 4*A2*A3*cosx3 - A3**2*cosx3**2))
     
    x7=        (A3*(((-(A2*A5) + A3**2*(1 + 4*NT)*cosx3**2)*sinx3 + 2*A2*A3*NT*sin2x3)*x4**2\
        + NT*(A3**2 - 2*A2*A5 + 8*A2*A3*cosx3 + A3**2*cos2x3)*sinx3*x4*x6 + (NT*(A3**2 - 2*A2*A5 + 8*A2*A3*cosx3\
        + A3**2*cos2x3)*sinx3*x6**2)/2. + (A4 - A3*cosx3**2)*((A2*(-1 + NT) + A5*NT - A3*(1 + 2*NT)*cosx3)*x4prime\
        + (A2*(-1 + NT) + A3*NT*cosx3)*x6prime)))/(NT*(A2*A5 - 4*A2*A3*cosx3 - A3**2*cosx3**2))
     
    x5=        (NT*(-2*A3*(2*A2 + A3*cosx3)*sinx3*(K*(A2*A5 - 4*A2*A3*cosx3 - A3**2*cosx3**2)*x3 + A3*(A2*A5 - A3**2*cosx3**2)*sinx3*x4**2\
         + A3*(A4 - A3*cosx3**2)*((A2 + A3*cosx3)*x4prime + A2*x6prime))*x6 + (A2*A5 - 4*A2*A3*cosx3 -  A3**2*cosx3**2)*(K*(A2*A5 - 4*A2*A3*cosx3 - A3**2*cosx3**2)* x6\
         + 2*A3*K*(2*A2 + A3*cosx3)*sinx3*x3*x6+ A3*cosx3*(A2*A5 - A3**2*cosx3**2)*x4**2* x6 +  A3**3*sinx3*sin2x3*x4**2*x6\
         +  A3**2*sin2x3*((A2 + A3*cosx3)*x4prime +  A2*x6prime)*x6 + 2*A3*(A2*A5 - A3**2*cosx3**2)* sinx3*x4*x4prime\
         + A3*(A4 - A3*cosx3**2)* (-(A3*sinx3*x4prime*x6) + (A2 + A3*cosx3)* x4second + A2*x6second))))/(K*(-(A2*A5) + 4*A2*A3*cosx3 + A3**2*cosx3**2)**2)
   
    
    x8=      -((JR1*(-((K*(-(A3*A4) + A2*JR1*(1 + NT) + A3*JR1*NT*cosx3 + A3**2*cosx3**2)*x2)/(A3*JR1*NT**2))\
             + (K*(-(A3*A4) + A2*JR1*(1 + NT) + A3*JR1*NT*cosx3 + A3**2*cosx3**2)*x3)/(A3*JR1*NT) + (A2 + A3*cosx3)*sinx3* x4**2\
             + 2*A2*sinx3*x4*x6 + A2*sinx3*x6**2 + (A2*x7)/A3 + (A4*NT*(4*A3*(2*A2 + A3*cosx3)*sinx3*x6*(-2*A3*(2*A2 + A3*cosx3)*sinx3*(K*(A2*A5 - 4*A2*A3*cosx3 -A3**2*cosx3**2)*x3\
             + A3*(A2*A5 - A3**2*cosx3**2)*sinx3*x4**2 + A3*(A4 - A3*cosx3**2)*((A2 + A3*cosx3)*x4prime + A2*x6prime))*x6\
             + (A2*A5 - 4*A2*A3*cosx3 - A3**2*cosx3**2)*(K*(A2*A5 - 4*A2*A3*cosx3 - A3**2*cosx3**2)*x6 + 2*A3*K*(2*A2 + A3*cosx3)*sinx3*x3*x6\
             + A3*cosx3*(A2*A5 - A3**2*cosx3**2)*x4**2*x6+ A3**3*sinx3*sin2x3*x4**2*x6 + A3**2*sin2x3*((A2 + A3*cosx3)*x4prime\
             + A2*x6prime)*x6+ 2*A3* (A2*A5 - A3**2*cosx3**2)*sinx3*x4* x4prime +  A3*(A4 - A3*cosx3**2)*(-(A3*sinx3*x4prime*x6)\
             + (A2 + A3*cosx3)*x4second + A2*x6second))) + (-(A2*A5) + 4*A2*A3*cosx3 + A3**2*cosx3**2)*(-2*A3*cosx3*(2*A2 + A3*cosx3)*(K*(A2*A5 - 4*A2*A3*cosx3 - A3**2*cosx3**2)*x3\
             + A3*(A2*A5 - A3**2*cosx3**2)*sinx3*x4**2 + A3*(A4 - A3*cosx3**2)*((A2 + A3*cosx3)*x4prime +  A2*x6prime))*x6**2\
             + 2*A3**2*sinx3**2*(K*(A2*A5 - 4*A2*A3*cosx3 -  A3**2*cosx3**2)*x3 + A3*(A2*A5 - A3**2*cosx3**2)*sinx3*x4**2 + A3*(A4 - A3*cosx3**2)*((A2 + A3*cosx3)*x4prime\
             + A2*x6prime))*x6**2 - 2*A3*(2*A2 + A3*cosx3)*sinx3* (K*(A2*A5 - 4*A2*A3*cosx3 - A3**2*cosx3**2)*x3 + A3*(A2*A5 - A3**2*cosx3**2)*sinx3*x4**2\
             + A3*(A4 - A3*cosx3**2)*((A2 + A3*cosx3)*x4prime +  A2*x6prime))*x6prime + ((A2*A5 - 4*A2*A3*cosx3 - A3**2*cosx3**2)*(A3* (32*A2*K*sinx3\
             + 8*A3*K*sin2x3 + 8*K*(2*A2*cosx3 + A3*cos2x3)*x3 + 2* (5*A3**2 - 2*A2*A5 + 9*A3**2*cos2x3)*sinx3*x4**2 +  3*A3**2*cosx3*x4prime -  4*A3*A4*cosx3*x4prime\
             + 8*A2*A3*cos2x3*x4prime + 9*A3**2*cos3x3*x4prime + 8*A2*A3*cos2x3*x6prime)*x6**2 - 4*A3*(A3**2 - 2*A2*A5\
             + A3**2*cos2x3)*sinx3* x4prime**2 -  2*A3*x6*(4*cosx3*(-A3**2 - 2*A2*A5 + 3*A3**2*cos2x3)*x4* x4prime -  2*A3*sinx3*((3*A3 - 2*A4 + 4*A2*cosx3\
             + 3*A3*cos2x3)*x4second + 4*A2*cosx3*x6second)) + 2*(2*A2*A5*K - 8*A2*A3*K*cosx3 -  2*A3**2*K*cosx3**2 + 4*A3*K*(2*A2 + A3*cosx3)* sinx3*x3 - A3*cosx3*(-A3**2 - 2*A2*A5\
             + 3*A3**2*cos2x3)*x4**2 + A3**2*(3*A3 - 2*A4 + 4*A2*cosx3 + 3*A3*cos2x3)*sinx3*x4prime + 2*A2*A3**2*sin2x3*x6prime) *x6prime -  4*A3*(A3**2 - 2*A2*A5\
             +  A3**2*cos2x3)*sinx3*x4*x4second + 4*A3*(A4 - A3*cosx3**2)* ((A2 + A3*cosx3)*x4third + A2*x6third)))/4.) ))/ (K*(-(A2*A5) + 4*A2*A3*cosx3 +  A3**2*cosx3**2)**3)\
             - (A3*NT*cosx3**2*(4*A3*(2*A2 + A3*cosx3)*sinx3*x6*(-2*A3*(2*A2 +A3*cosx3)*sinx3*(K*(A2*A5 - 4*A2*A3*cosx3 -A3**2*cosx3**2)*x3 + A3*(A2*A5 - A3**2*cosx3**2)*sinx3*x4**2\
             + A3*(A4 - A3*cosx3**2)*((A2 + A3*cosx3)*x4prime + A2*x6prime))*x6 + (A2*A5 - 4*A2*A3*cosx3 - A3**2*cosx3**2)*(K*(A2*A5 - 4*A2*A3*cosx3 - A3**2*cosx3**2)*x6\
             + 2*A3*K*(2*A2 + A3*cosx3)*sinx3*x3*x6 + A3*cosx3*(A2*A5 - A3**2*cosx3**2)*x4**2*x6+ A3**3*sinx3*sin2x3*x4**2* x6 + A3**2*sin2x3*((A2 + A3*cosx3)*x4prime\
             + A2*x6prime)*x6 + 2*A3* (A2*A5 - A3**2*cosx3**2)*sinx3*x4*x4prime + A3*(A4 - A3*cosx3**2)*(-(A3*sinx3*x4prime*x6) + (A2 + A3*cosx3)*x4second +  A2*x6second)))\
             + (-(A2*A5) + 4*A2*A3*cosx3 + A3**2*cosx3**2)*(-2*A3*cosx3*(2*A2 + A3*cosx3)*(K*(A2*A5 - 4*A2*A3*cosx3 - A3**2*cosx3**2)*x3 + A3*(A2*A5 - A3**2*cosx3**2)*sinx3*x4**2\
             + A3*(A4 - A3*cosx3**2)*((A2 + A3*cosx3)*x4prime + A2*x6prime))*x6**2 + 2*A3**2*sinx3**2*(K*(A2*A5 - 4*A2*A3*cosx3 - A3**2*cosx3**2)*x3\
             +  A3*(A2*A5 - A3**2*cosx3**2)* sinx3*x4**2 + A3*(A4 - A3*cosx3**2)*((A2 + A3*cosx3)*x4prime +  A2*x6prime))* x6**2 -  2*A3*(2*A2\
             + A3*cosx3)*sinx3*(K*(A2*A5 - 4*A2*A3*cosx3 - A3**2*cosx3**2)*x3 +  A3*(A2*A5 - A3**2*cosx3**2)*sinx3*x4**2 +  A3*(A4 - A3*cosx3**2)* ((A2 + A3*cosx3)*x4prime\
             +  A2*x6prime))*x6prime + ((A2*A5 - 4*A2*A3*cosx3 -  A3**2*cosx3**2)*(A3*(32*A2*K*sinx3 +  8*A3*K*sin2x3 +  8*K*(2*A2*cosx3 +  A3*cos2x3)*x3 +  2*(5*A3**2 - 2*A2*A5\
             +  9*A3**2*cos2x3)*sinx3*x4**2 + 3*A3**2*cosx3*x4prime -  4*A3*A4*cosx3*x4prime + 8*A2*A3*cos2x3*x4prime +  9*A3**2*cos3x3*x4prime\
             + 8*A2*A3*cos2x3*x6prime)*x6**2 - 4*A3*(A3**2 - 2*A2*A5 +  A3**2*cos2x3)*sinx3*x4prime**2 -  2*A3*x6*(4*cosx3*(-A3**2 - 2*A2*A5\
             +  3*A3**2*cos2x3)*x4* x4prime - 2*A3*sinx3*((3*A3 - 2*A4 +  4*A2*cosx3 + 3*A3*cos2x3)*x4second + 4*A2*cosx3*x6second))\
             +  2*(2*A2*A5*K -  8*A2*A3*K*cosx3 -  2*A3**2*K*cosx3**2 +  4*A3*K*(2*A2 + A3*cosx3)* sinx3*x3 - A3*cosx3*(-A3**2 - 2*A2*A5 +  3*A3**2*cos2x3)*x4**2\
             + A3**2* (3*A3 - 2*A4 +  4*A2*cosx3 +  3*A3*cos2x3)*sinx3*x4prime +  2*A2*A3**2*sin2x3*x6prime)*x6prime -  4*A3*(A3**2 - 2*A2*A5 +  A3**2*cos2x3)*sinx3*x4*x4second\
             + 4*A3*(A4 - A3*cosx3**2)*((A2 + A3*cosx3)*x4third +  A2*x6third)))/4.)))/(K*(-(A2*A5) + 4*A2*A3*cosx3 +  A3**2*cosx3**2)**3)))/ (-A4 + A3*cosx3**2))  
    
    U1=x7+x8
    U2=x8
    if type(t) is algopy.utpm.utpm.UTPM:
        X=zeros(8,dtype=t) 
        X[0]=x1
        X[1]=x2
        X[2]=x3
        X[3]=x4
        X[4]=x5
        X[5]=x6
        X[6]=U1
        X[7]=U2  
    else:
        X=array([x1,x2,x3,x4,x5,x6,U1,U2])
    
    return X

if __name__=="__main__":       
    #from matplotlib import use
    # choose your backend
    #use('WxAgg')
    #use('Gtk3Agg')
    #use('TkAgg')
    #use('Qt4Agg')
    from numpy import shape,arange
    from matplotlib.pyplot import plot,show,legend,figure,ylabel,xlabel,yscale,yticks
    
    tlist=npzeros(100)
    t0=0.
    T=2.4
    
    steps=100
    h=(T-t0)/steps
    tlist=arange(t0,T+h,h)
    yxout=RoboticArmSolutionDeLu87(tlist)
    print 'solution ',shape(yxout),yxout
    
    figure(0)
    plot(tlist,yxout[0],label='x1')
    #plot(tlist,yxout[1],label='x2')
    plot(tlist,yxout[2],label='x3')
    plot(tlist,yxout[3],label='x4')
    #plot(tlist,yxout[4],label='x5')
    plot(tlist,yxout[5],label='x6')
    #plot(tlist,yxout[6],label='u1')
    #plot(tlist,yxout[7],label='u2')
    #plot(tlist,log(-yxout[7]),label='u2')
    
    legend(loc='upper left', shadow=True)
    ylabel('solution')
    xlabel('t')
    
      
    figure(1)
    #plot(tlist,yxout[0],label='x1')
    #plot(tlist,vorzeichen(yxout[1]),label='log(x2),-log(-x2)')
    plot(tlist,yxout[1],label='x2')
    #plot(tlist,yxout[2],label='x3')
    #plot(tlist,yxout[3],label='x4')
    plot(tlist,(yxout[4]),label='x5')
    #plot(tlist,yxout[5],label='x6')
    plot(tlist,yxout[6],label='u1')
    plot(tlist,yxout[7],label='u2')
    #plot(tlist,log(-yxout[7]),label='u2')
    
    legend(loc='upper left', shadow=True)
    ylabel('solution')
    xlabel('t')
    yscale('symlog')
    yticks([-10**8,-10**4,0,10**4,10**8])
    show()