###################
# The Lorenz System      #
# Controller type: RFPT  #
# MIMO System              #
##################

####################
# The equation of motion #
####################

##########
# ẋ=σ(y-x)    #
# ẏ=x(ρ-z)-y #
# ż=xy-βz    #
##########


#####################
# Functions for FPT #
#####################

function KB(n,lambda,errors,nominal)
  out=0
  for l=0:n-1
    out=out+(factorial(n)/(factorial(l)*factorial(n-l))*errors[l+1]*lambda^(n-l))
  end
  out=out+nominal
  return out
end
# Simple Euler integral (rectangle method)
function Integ(Integral,step,input)
    out=Integral+step*input
    return out
end

# Simple Sigmoid function, mostly you can use any Sigmoid type function, like tanh()
function sigmoid(x)
    s=x/(1+abs(x))
  return s
end
# The Original Deformation function for SISO case (note here I used tanh() for sigmoid function)
function G_SISO(past_input,past_response,desired,Kc,Bc,Ac)
  out=(Kc+past_input)*(1 + Bc * tanh(Ac * (past_response - desired))) - Kc
  return out
end

# The Original Deformation function for MIMO case
function G_MIMO(past_input,past_response,desired,error_limit,Kc,Bc,Ac) # Inputs and Outputs are vectors
#  need control parameters  K B A
    error_norm=vecnorm(past_response - desired,2)
    # If the norm of the error is greater then the limnit compute the deformation
    # (it is not near the Fixed Point)
  if error_norm>error_limit
    e_direction=(past_response-desired)/error_norm
    B_factor= Bc* sigmoid(Ac *error_norm)
    out=(1+B_factor)*past_input +B_factor*Kc*e_direction
  else
    out=past_input # Almost in the Fixed Point
  end
  return out
end

#################
# Control Parameters #
#################
Adaptive=1

K=-1e4
B=-1  #±1
A=1/(100*K)

#############################################
# Parameters to design a Nominal Trajectory #
#############################################

A₁=2
ω₁=0.5
A₂=3
ω₂=0.7
A₃=1
ω₃=1

#################
# Time variable #
#################

# cycle time in seconds
δt=1e-3
# The lenght of the simulation in seconds
LONG=Int(2e4)

Λ=32
Λ1=3

d1=2
d2=4
d3=5

dd1=3
dd2=7
dd3=9

l1=0.15
l2=0.10
l3=0.30

##########################
# Exact model parameters #
##########################
σₑ=5
βₑ=8/3
ρₑ=40

############################
# Approx. model parameters #
############################
σₐ=4
βₐ=7/3
ρₐ=36

#Arrays
xN=zeros(LONG)
xN_p=zeros(LONG)
xN_pp=zeros(LONG)
yN=zeros(LONG)
yN_p=zeros(LONG)
yN_pp=zeros(LONG)
zN=zeros(LONG)
zN_p=zeros(LONG)
zN_pp=zeros(LONG)

x_p_des=zeros(LONG)
y_p_des=zeros(LONG)
z_p_des=zeros(LONG)

u_x=zeros(LONG)
u_y=zeros(LONG)
u_z=zeros(LONG)

t=zeros(LONG)

y_Def_pp=zeros(LONG)
Def=zeros(LONG)

x=zeros(LONG)
y=zeros(LONG)
z=zeros(LONG)
x_p=zeros(LONG)
y_p=zeros(LONG)
z_p=zeros(LONG)
x_pp=zeros(LONG)
y_pp=zeros(LONG)
z_pp=zeros(LONG)
enorm=zeros(LONG)
ed_x=zeros(LONG)
ed_y=zeros(LONG)
ed_z=zeros(LONG)
bf=zeros(LONG)

qdef_x=zeros(LONG)
qdef_y=zeros(LONG)
qdef_z=zeros(LONG)

def_err_x=0
def_err_y=0
def_err_z=0
#initial conditions
hint_x=0
hint_y=0
hint_z=0

past_input=[0,0,0]
past_response=[0,0,0]
error_limit=1e-3
#Simulation

for i=1:LONG-1
  t[i]=δt*i
  #Nominal Trajectory
  xN[i]=A₁*sin(ω₁*t[i])
  xN_p[i]=A₁*ω₁*cos(ω₁*t[i])
  xN_pp[i]=-A₁*ω₁^2*sin(ω₁*t[i])

  yN[i]=A₂*sin(ω₂*t[i])
  yN_p[i]=A₂*ω₂*cos(ω₂*t[i])
  yN_pp[i]=-A₂*ω₂^2*sin(ω₂*t[i])

  zN[i]=A₃*sin(ω₃*t[i])
  zN_p[i]=A₃*ω₃*cos(ω₃*t[i])
  zN_pp[i]=-A₃*ω₃^2*sin(ω₃*t[i])
  #Errors
  h_x=xN[i]-x[i]
  h_y=yN[i]-y[i]
  h_z=zN[i]-z[i]

  hint_x=hint_x+δt*h_x
  hint_y=hint_y+δt*h_y
  hint_z=hint_z+δt*h_z

  h_p_x=xN_p[i]-x_p[i]
  h_p_y=yN_p[i]-y_p[i]
  h_p_z=zN_p[i]-z_p[i]
# the error vector
 errors_x=[hint_x,h_x]
 errors_y=[hint_y,h_y]
 errors_z=[hint_z,h_z]
  x_p_des[i]=KB(2,Λ,errors_x,xN_p[i])
  y_p_des[i]=KB(2,Λ,errors_y,yN_p[i])
  z_p_des[i]=KB(2,Λ,errors_z,zN_p[i])
  desired=[x_p_des[i],y_p_des[i], z_p_des[i]]
  #deformation
  if Adaptive==1 && i>10
  past_input=G_MIMO(past_input,past_response,[x_p_des[i],y_p_des[i], z_p_des[i]],error_limit,K,B,A)
 else
   past_input=desired
 end
 #qdef_p_x[i+1]=qDef_pp_mem[1]+δt*qdef_p_x[i]
 qdef_x[i+1]=past_input[1]+δt*qdef_x[i]
 def_err_x=def_err_x+δt*(qdef_x[i]-x[i])
 u_x[i]=(Λ1^2*def_err_x+2*Λ1*(qdef_x[i]-x[i])+past_input[1])/dd1

 qdef_y[i+1]=past_input[2]+δt*qdef_y[i]
 def_err_y=def_err_y+δt*(qdef_y[i]-y[i])
 u_y[i]=(Λ1^2*def_err_y+2*Λ1*(qdef_y[i]-y[i])+past_input[2])/dd2

 qdef_z[i+1]=past_input[3]+δt*qdef_z[i]
 def_err_z=def_err_z+δt*(qdef_z[i]-z[i])
 u_z[i]=(Λ1^2*def_err_z+2*Λ1*(qdef_z[i]-z[i])+past_input[3])/dd3
 # u_x[i]=l1*(x_p_des[i]-σₐ*(y[i]-x[i]))/dd1
 # u_y[i]=l2*(y_p_des[i]-x[i]*(ρₐ-z[i]))/dd2
 # u_z[i]=l3*(z_p_des[i]-x[i]*y[i]+βₐ*z[i])/dd3

 x_p[i]=σₑ*(y[i]-x[i])+d1*u_x[i]
 y_p[i]=x[i]*(ρₑ-z[i])-y[i]+d2*u_y[i]
 z_p[i]=x[i]*y[i]-βₑ*z[i]+d3*u_z[i]
 past_response=[x_p[i],y_p[i],z_p[i]]


 x[i+1]=Integ(x[i],δt,x_p[i])
 y[i+1]=Integ(y[i],δt,y_p[i])
 z[i+1]=Integ(z[i],δt,z_p[i])

end #For
#ppp=zeros(3)

using PyPlot
figure(1)
grid("on")
title("Control Signals")
# xlabel(L"Time $[s]$")
# ylabel(L"q $[m]$")
#Nominal
# plot(t[3:LONG-1],xN_p[3:LONG-1],color="#7684FF")
# plot(t[3:LONG-1],yN_p[3:LONG-1],color="#2A40FF")
# plot(t[3:LONG-1],zN_p[3:LONG-1],color="#3B427F")
#Realized
plot(t[3:LONG-1],u_x[3:LONG-1],color="#FFAA41","r--")
plot(t[3:LONG-1],u_y[3:LONG-1],color="#FF9F28","r--")
plot(t[3:LONG-1],u_z[3:LONG-1],color="#B2690E","r--")

figure(2)
title("Nominal and Simulated")
plot(t[3:LONG-1],xN_p[3:LONG-1],color="#7684FF")
plot(t[3:LONG-1],yN_p[3:LONG-1],color="#2A40FF")
plot(t[3:LONG-1],zN_p[3:LONG-1],color="#3B427F")
#Realized
plot(t[3:LONG-1],x_p[3:LONG-1],color="#FFAA41","r--")
plot(t[3:LONG-1],y_p[3:LONG-1],color="#FF9F28","r--")
plot(t[3:LONG-1],z_p[3:LONG-1],color="#B2690E","r--")
figure(3)
grid("on")
title("Tracking Error")
xlabel(L"Time $[s]$")
ylabel(L"error $[m]$")
plot(t[3:LONG-1],xN[3:LONG-1]-x[3:LONG-1],color="red")
plot(t[3:LONG-1],yN[3:LONG-1]-y[3:LONG-1],color="green")
plot(t[3:LONG-1],zN[3:LONG-1]-z[3:LONG-1],color="blue")