# Hang Glider (hang)
# Objective is to maximize the final horizontal distance a glider flies.

# Ref: "Combining Direct and Indirect Methods in Optimal Control: Range
# Maximization of a Hang Glider", R. Bulirsch, E. Nerz, H.J. Pesch, 
# and O. von Stryk, in "Optimal Control: Calculus of Variations, Optimal
# Control Theory and Numerical Methods", ed. by R. Bulirsch, A. Miele, J.
# Stoer, and K.H. Well (1993) Birkhauser

# An incorrect formulation of this problem appears, with the same name,
# in Jorge More's COPS suite of problems.

# NOTES:
#	Midpoint Discretization 
#	Uniform time step;

param N;
set POS_NODES := {0..N};
set VEL_NODES := {0..N-1};
set ACC_NODES := {1..N-1};

param x_0;
param y_0;
param vx_0;
param vy_0;
param cL_0;
param x_n;
param y_n;
param vx_n;
param vy_n;
param cL_n;

param t0;
param cL_min;
param cL_max;
param uM;
param R;
param c0;
param k;
param m;
param S;
param rho;
param g;

param W := m*g;

var T >= 0;

# State Variables
var x {i in POS_NODES} >= 0;
var y {i in POS_NODES} >= 0;
var vx {i in VEL_NODES} = N*(x[i+1]-x[i])/T;
var vy {i in VEL_NODES} = N*(y[i+1]-y[i])/T;
var vx_dot {i in ACC_NODES} = N*(vx[i] - vx[i-1])/T;
var vy_dot {i in ACC_NODES} = N*(vy[i] - vy[i-1])/T;

# Control variable
var cL {i in ACC_NODES} >= cL_min, <= cL_max;

# Dummy Variables
var cD {i in ACC_NODES} = c0+k*cL[i]^2;
#var X {i in POS_NODES} = (x[i]/R - 2.5)^2;
#var ua {i in POS_NODES} = uM*(1-X[i])*exp(-X[i]);
var ua {i in POS_NODES} = 0;	# upward air motion
var ha {i in POS_NODES} = -5*(1+atan((y[i]-30)/10)); # horizontal air motion
var Vy {i in ACC_NODES} = (vy[i]+vy[i-1])/2 - ua[i];
var Vx {i in ACC_NODES} = (vx[i]+vx[i-1])/2 - ha[i];
var vr {i in ACC_NODES} = sqrt(Vx[i]^2 + Vy[i]^2);
var D {i in ACC_NODES} = .5*cD[i]*rho*S*vr[i]^2;
var L {i in ACC_NODES} = .5*cL[i]*rho*S*vr[i]^2;
var sin_eta {i in ACC_NODES} = Vy[i]/vr[i];
var cos_eta {i in ACC_NODES} = Vx[i]/vr[i];

maximize final_x: x[N];

s.t. newt_x{i in ACC_NODES}: 
    vx_dot[i] = (-L[i]*sin_eta[i] - D[i]*cos_eta[i])/m;
s.t. newt_y{i in ACC_NODES}: 
    vy_dot[i] = (L[i]*cos_eta[i] - D[i]*sin_eta[i] - W)/m;

# Boundary Conditions
subject to x_ic : x[0] = x_0;
subject to y_ic : y[0] = y_0;
subject to vx_ic: vx[0] = vx_0;
subject to vy_ic: vy[0] = vy_0;

subject to y_fc : y[N] = y_n;
subject to vx_fc: vx[N-1] = vx_n;
subject to vy_fc: vy[N-1] = vy_n;

#subject to y_fc : y[N] >= y_n;
#subject to vx_fc: vx[N-1] >= vx_n;
#subject to vy_fc: vy[N-1] >= vy_n;

#subject to monotone_x {i in VEL_NODES}: x[i+1] >= x[i];
subject to novomit_x {i in ACC_NODES}: -3 <= vx_dot[i] <= 3;
subject to novomit_y {i in ACC_NODES}: -3 <= vy_dot[i] <= 3;

# Hang Glider Problem (hang)
# Data which needs to be reinitialized

let N := 200;

let x_0 := 0;
let y_0 := 100;
let vx_0 := 13.2275675 - 2.5*(1+atan((y_0-30)/10)); # horizontal air motion
let vy_0 := -1.28750052;

let y_n := 0;
let vx_n := 13.2275675;
let vy_n := 0;

let uM := 2.5;
let R := 100;
let c0 := 0.034;
let k := 0.069662;
let m := 100;
let S := 14;
let rho := 1.13;
let g := 9.80665;
let t0 := 0;
let cL_min := 0;
let cL_max := 1.4;
#let cL_max := 2.8;  

# initial guess
let {j in POS_NODES} x[j] := 5000*j/N;
let {j in POS_NODES} y[j] := -100*j/N+100;
let {j in ACC_NODES} cL[j] := .7;
let T := 30;

option show_stats 1;

solve;

display x, y, vx, vy;
display T;

printf {j in POS_NODES}: "%10f %10f \n", x[j], y[j] > y_vs_x;

printf {j in POS_NODES}: "%10f %10f \n", j*T/N, x[j] > x_vs_t;

printf {j in POS_NODES}: "%10f %10f \n", j*T/N, y[j] > y_vs_t;

printf {j in VEL_NODES}: "%10f %10f \n", (j+0.5)*T/N, vx[j] > vx_vs_t;

printf {j in VEL_NODES}: "%10f %10f \n", (j+0.5)*T/N, vy[j] > vy_vs_t;

printf {j in ACC_NODES}: "%10f %10f \n", j*T/N, cL[j] > cL_vs_t;

printf {j in POS_NODES}: "%10f %10f \n", x[j], ua[j] > ua_vs_x;

printf {j in ACC_NODES}: "%10f %10f \n", j*T/N, newt_x[j] > newtx_vs_t;

printf {j in ACC_NODES}: "%10f %10f \n", j*T/N, newt_y[j] > newty_vs_t;