# Hang Glider (hang)
# Objective is to maximize the final horizotal 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
                                                                               
# NOTES:
#	Trapezoidal Discretization 
#	Uniform time step;
#	Solvers ability to converge seems highly dependent upon the intial
#		guess.

param N;
set INT_NODES := {1..N};
set ALL_NODES := {0..N};


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 cL_min;
param cL_max;
param c0;
param k;
param m;
param S;
param rho;
param g;

param W := m*g;

# State Variables
var x {i in ALL_NODES};
var y {i in ALL_NODES};
var vx {i in ALL_NODES} <= 30, >= -30;
var vy {i in ALL_NODES} <= 30, >= -30;

# Control and dummy timestep variable
var cL {i in ALL_NODES} >= cL_min, <= cL_max;
var T >= 10, <= 200;

# Dummy Variables
var cD {i in ALL_NODES} = c0+k*cL[i]^2;
var vr {i in ALL_NODES} = sqrt(vx[i]^2 + vy[i]^2);
var D {i in ALL_NODES} = .5*cD[i]*rho*S*vr[i]^2;
var L {i in ALL_NODES} = .5*cL[i]*rho*S*vr[i]^2;
var sin_eta {i in ALL_NODES} = vy[i]/vr[i];
var cos_eta {i in ALL_NODES} = vx[i]/vr[i];
var vx_dot {i in ALL_NODES} = (-L[i]*sin_eta[i] - D[i]*cos_eta[i])/m;
var vy_dot {i in ALL_NODES} = (L[i]*cos_eta[i] - D[i]*sin_eta[i] - W)/m;

maximize final_x: x[N];
#minimize init_x: x[0];

# Trapezoidal Discretization
subject to x_eqn {j in INT_NODES}:
   (x[j] - x[j-1])/(T/N) = .5*(vx[j] + vx[j-1]);

subject to y_eqn {j in INT_NODES}:
   (y[j] - y[j-1])/(T/N) = .5*(vy[j] + vy[j-1]);

subject to vx_eqn {j in INT_NODES}:
   (vx[j] - vx[j-1])/(T/N) = .5*(vx_dot[j] + vx_dot[j-1]);

subject to vy_eqn {j in INT_NODES}:
   (vy[j] - vy[j-1])/(T/N) = .5*(vy_dot[j] + vy_dot[j-1]);

# 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 x_fc : x[N] = x_0;
subject to y_fc : y[N] = y_n;
subject to vx_fc1: vx[N] = vx_n;
subject to vy_fc1: vy[N] = vy_n;

subject to monotone_x {i in 1..N}: x[i] >= x[i-1];
subject to novomit_x {i in ALL_NODES}: -3 <= vx_dot[i] <= 3;
subject to novomit_y {i in ALL_NODES}: -3 <= vy_dot[i] <= 3;

#subject to dofless {i in 0..1}: cL[i] = cL[i+1];

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

let N := 150;

data;

param x_0 := 0;
param y_0 := 1000;
param vx_0 := 13.2275675;
param vy_0 := -1.28750052;

param y_n := 900;
param vx_n := 13.2275675;
param vy_n := -1.28750052;

param c0 := 0.034;
param k := 0.069662;
param m := 100;
param S := 14;
param rho := 1.13;
param g := 9.80665;
param cL_min := 0;
param cL_max := 1.4;

# initial guess
let {j in ALL_NODES} x[j] := 5000*j/N;
#let {j in ALL_NODES} x[j] := 0;
let {j in ALL_NODES} y[j] := -100*j/N+1000;
let {j in ALL_NODES} vx[j] := 13.2275675;
let {j in ALL_NODES} vy[j] := -1.28760052;
let {j in ALL_NODES} cL[j] := 0.7;
let T := 30;

solve;

display T;

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

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

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

printf {j in ALL_NODES}: "%10f %10f \n", j*T/N, vx[j] > vx_vs_t;

printf {j in ALL_NODES}: "%10f %10f \n", j*T/N, vy[j] > vy_vs_t;

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

printf {j in ALL_NODES}: "%10f %10f \n", x[j], cL[j] > cL_vs_x;

#let {j in ALL_NODES} cL[j] := .7;

printf {j in INT_NODES}: "%10f %10f %10f   %10f %10f %10f \n",
   (vx[j] - vx[j-1])/(T/N), .5*(vx_dot[j] + vx_dot[j-1]),
   (vx[j] - vx[j-1])/(T/N) - .5*(vx_dot[j] + vx_dot[j-1]),
   (vy[j] - vy[j-1])/(T/N), .5*(vy_dot[j] + vy_dot[j-1]),
   (vy[j] - vy[j-1])/(T/N) - .5*(vy_dot[j] + vy_dot[j-1]);