using JuMP, Ipopt, DelimitedFiles

tol_fun=1e-13
mu=1e3

mod = Model(with_optimizer(Ipopt.Optimizer, max_iter=2000,tol=tol_fun,acceptable_tol=tol_fun, print_level=5))

# Parameters

N=100
hor_pos=1.0;
pi=3.141592;
W=10;
Ldim=40;
m=86;
alphaval=20
Tmax=260;
rpmmax=240;
Dev=8.5;
omegamax=rpmmax/60;
c = 0.22;
g=9.81;
Fmax=2*pi*Tmax/Dev;
Vmax=Dev*omegamax;
tau=sqrt(m*Ldim/Fmax)
vmax=Vmax*tau/Ldim;
beta=m*g/Fmax
rho=1.225;
A=1;
dtmin=0.0001;
dtmax=1;
nlim=W/Ldim
C=rho*c*A*Ldim/(2*m)
nmin=0.0001
beta=m*g/Fmax
r0=1+hor_pos*nlim

###################################
# Initial guess
###################################

x_old_1=ones(1,N)
y_old_1=ones(1,N)
xdot_old_1=ones(1,N)
ydot_old_1=ones(1,N)
Fx_old_1=ones(1,N)
Fy_old_1=ones(1,N)
dt_old_1=0.01

###################################
# Compile variables
###################################

@variable(mod, x_1[i=1:N], start=x_old_1[i])
@variable(mod, xdot_1[i=1:N], start=xdot_old_1[i])
@variable(mod, y_1[i=1:N]>=0, start=y_old_1[i])
@variable(mod, ydot_1[i=1:N]>=0, start=ydot_old_1[i])
@variable(mod, dtmin<=dt_1<=dtmax , start = dt_old_1[1])
@variable(mod, Fx_1[i=1:N], start=Fx_old_1[i])


@NLexpression(mod, t_f_1, dt_1*N)

@NLexpression(mod, n_1[i=1:N],  x_1[i]-1 )
@NLexpression(mod, s_1[i=1:N],  y_1[i] )

@NLexpression(mod, alpha_1[i=1:N],  tan(pi/180*alphaval) )
@NLexpression(mod, alphadash_1[i=1:N],  0)

@NLexpression(mod, dsdx_1[i=1:N], 0)
@NLexpression(mod, dsdy_1[i=1:N], 1)
@NLexpression(mod, dndx_1[i=1:N], 1)
@NLexpression(mod, dndy_1[i=1:N], 0)

@NLexpression(mod, dfdx_1[i=1:N], alpha_1[i]*dndx_1[i]+alphadash_1[i]*dsdx_1[i]*n_1[i])
@NLexpression(mod, dfdy_1[i=1:N], alpha_1[i]*dndy_1[i]+alphadash_1[i]*dsdy_1[i]*n_1[i])

@NLexpression(mod, v[i=1:N], sqrt(xdot_1[i]^2*(1+tan(pi/180*alphaval)^2)+ydot_1[i]^2))
@NLexpression(mod, Fy_1[i=1:N], 0)

@NLexpression(mod, Fxproj_1[i=1:N], Fy_1[i]*xdot_1[i]/v[i]+ cos(pi/180*alphaval)*Fx_1[i]*ydot_1[i]/v[i])
@NLexpression(mod, Fyproj_1[i=1:N], Fy_1[i]*ydot_1[i]/v[i]- 1/cos(pi/180*alphaval)*Fx_1[i]*xdot_1[i]/v[i])

@NLexpression(mod, ODE1_1[i=2:N],  ((x_1[i]-x_1[i-1])/dt_1-xdot_1[i]) )
@NLexpression(mod, ODE2_1[i=2:N],  ((y_1[i]-y_1[i-1])/dt_1-ydot_1[i]) )
@NLexpression(mod, ODE3_1[i=3:N],  (ydot_1[i]-ydot_1[i-1])/dt_1 - (Fyproj_1[i]-beta*dfdy_1[i]/sqrt(1+dfdy_1[i]^2+dfdx_1[i]^2) )  )
@NLexpression(mod, ODE4_1[i=3:N],  ((xdot_1[i]-xdot_1[i-1])/dt_1 - (Fxproj_1[i]-cos(pi/180*alphaval)*beta*dfdx_1[i]/sqrt(1+dfdy_1[i]^2+dfdx_1[i]^2))) )


###################################
# Solution
###################################

@NLexpression(mod, con_1, (x_1[1]-r0)^2+(y_1[1])^2+(y_1[N]-1)^2+ (ydot_1[2])^2+(xdot_1[2])^2+sum(ODE1_1[i]^2+ODE2_1[i]^2 for i=2:N)+sum(ODE3_1[i]^2+ODE4_1[i]^2 for i=3:N))

@NLexpression(mod, my_obj, t_f_1 + mu*(con_1) )

@NLobjective(mod, Min, my_obj )

optimize!(mod)

println(sqrt((objective_value(mod)-value(t_f_1))/mu)/(N))

######################
# Save data
######################

writedlm(string("data_passive/Fmax"),Fmax)
writedlm(string("data_passive/x_1_",Int64(round(alphaval))),value.(x_1))
writedlm(string("data_passive/y_1_",Int64(round(alphaval))),value.(y_1))
writedlm(string("data_passive/t_f_1_",Int64(round(alphaval))),value(t_f_1))
writedlm(string("data_passive/dt_1_",Int64(round(alphaval))),value(dt_1))