# Modified Zermelo problem
# Two-phase-problem
# as of S.O. Erb: Mit dem Schiff von A nach B
# unpublished report
# Institute of Flight Mechanics and Flight Control 
# University of Stuttgart
# implemented by
# Dieter Kraft
# Munich University of Applied Sciences
# September 2004

param pi := 4*atan(1);
param n1;
param n2;
param p{1..8};
param u1l;
param u1u;
param u2l;
param u2u;

var t1 >= 0.1, <= 6.5, :=5.50; #t*=4.20078 for n1=25
var t2 >= 6.6, <= 22., :=8.95; #t*=7.54770 for n2=25

var h1 = t1/n1/2;
var h2 = (t2-t1)/n2/2;

var u1{0..n1};
var u2{0..n2};

var x1{1..4,0..n1};
var x2{1..4,0..n2};



minimize final_time: t2-t1 
+ 1d-8*sum{i in 0..n2-1} (u2[i+1]-u2[i])^2/h2;

# Differential constraints (trapezoidal discretization)

de1{i in 0..n1-1}: x1[1,i+1] = x1[1,i] 
+ h1*p[1]*((1-(2/pi*x1[4,i])^2)*cos(x1[3,i])-0.7*x1[2,i]
+ (1-(2/pi*x1[4,i+1])^2)*cos(x1[3,i+1])-0.7*x1[2,i+1]);
de2{i in 0..n1-1}: x1[2,i+1] = x1[2,i] 
+ h1*p[1]*((1-(2/pi*x1[4,i])^2)*sin(x1[3,i])
+ (1-(2/pi*x1[4,i+1])^2)*sin(x1[3,i+1]));
de3{i in 0..n1-1}: x1[3,i+1] = x1[3,i] 
+ h1*(sin(2*x1[4,i])+sin(2*x1[4,i+1]));
de4{i in 0..n1-1}: x1[4,i+1] = x1[4,i] 
+ h1*(u1[i]+u1[i+1]);

de5{i in 0..n2-1}: x2[1,i+1] = x2[1,i] 
+ h2*p[1]*((1-(2/pi*x2[4,i])^2)*cos(x2[3,i])-0.7*x2[2,i]
+ (1-(2/pi*x2[4,i+1])^2)*cos(x2[3,i+1])-0.7*x2[2,i+1]);
de6{i in 0..n2-1}: x2[2,i+1] = x2[2,i] 
+ h2*p[1]*((1-(2/pi*x2[4,i])^2)*sin(x2[3,i])
+ (1-(2/pi*x2[4,i+1])^2)*sin(x2[3,i+1]));
de7{i in 0..n2-1}: x2[3,i+1] = x2[3,i] 
+ h2*(sin(2*x2[4,i])+sin(2*x2[4,i+1]));
de8{i in 0..n2-1}: x2[4,i+1] = x2[4,i] 
+ h2*(u2[i]+u2[i+1]);

# Algebraic state constraints

ae1{i in 0..n1}: (x1[1,i]-4.0)^2 + (x1[2,i]-1.1)^2  >= p[6]^2;
ae2{i in 0..n2}: (x2[1,i]+1.2)^2 + (x2[2,i]-p[7])^2 >= 1;
ae3{i in 0..n1}:    1 <= x1[1,i] <= 5;
ae4{i in 0..n1}: p[3] <= x1[2,i] <= 1;
ae5{i in 0..n2}: p[4] <= x2[1,i] <= 2;
ae6{i in 0..n2}:    0 <= x2[2,i] <= 3;

# Control constraints

ce1{i in 0..n1}: u1l <= u1[i] <= u1u;
ce2{i in 0..n2}: u2l <= u2[i] <= u2u;

# Initial conditions phase I

ic1: x1[1,0] = p[2];
ic2: x1[2,0] = p[3]; 
ic3:  0.0 <= x1[3,0] <= 3.3;
ic4: -1.7 <= x1[4,0] <= 1.7;

# Final conditions phase I = initial conditions phase II
# Encounter with support ship

ic5: 2.0-0.15*p[1]*p[8]*t1 = x1[1,n1];
ic6: 3.0-p[1]*p[8]*t1      = x1[2,n1];

# Final conditions phase II

fc1: x2[1,n2] = p[4];
fc2: x2[2,n2] = p[5];

# Phase connecting conditions

ph1: x2[1,0] = x1[1,n1];
ph2: x2[2,0] = x1[2,n1];
ph3: x2[3,0] = x1[3,n1];
ph4: x2[4,0] = x1[4,n1];

# Data

# Parameter values

let n1   := 25;
let n2   := 2500;
let p[1] := 1.00;
let p[2] := 3.66;
let p[3] :=-1.86;
let p[4] :=-3.00;
let p[5] := 1.80;
let p[6] := 1.28;
let p[7] := 0.90;
let p[8] := 0.55;
let u1l  := -1.0;
let u1u  :=  1.0;
let u2l  := -1.0;
let u2u  :=  1.0;

# Initial estimates of position

let {i in 0..n1} x1[1,i] := p[2]+(1.5-p[2])*i/n1;
let {i in 0..n1} x1[2,i] := p[3]+(.75-p[3])*i/n1;
let {i in 0..n1} x1[3,i] := 1.50;
let {i in 0..n1} x1[4,i] := 0.43;
let {i in 0..n2} x2[1,i] := 1.5+(p[4]-1.5)*i/n2;
let {i in 0..n2} x2[2,i] := .75+(p[5]-.75)*i/n2;

# and control

let {i in 0..n1} u1[i] := -0.30;
let {i in 0..n2} u2[i] := -0.45;

option loqo_options  'iterlim=10000 outlev=1';
option lancelot_options 'outlev=1';
option solver ipopt;
#option solver lancelot;
#option solver loqo;
#option solver knitro;

solve;

display _total_solve_time, t1, t2, ic5.slack, ic5.slack, ph1.slack, ph2.slack, ph3.slack, ph4.slack;
printf {i in 0..n1}: 
"%10.5f %10.5f %10.5f %10.5f %10.5f %10.5f %10.5f %10.5f \n",
i/n1*t1, x1[1,i], x1[2,i], x1[3,i], x1[4,i], u1[i], 4.0-i/n1*p[6], 1.1-sqrt(p[6]^2-(i/n1*p[6])^2) > z;
printf {i in 0..n2}: 
"%10.5f %10.5f %10.5f %10.5f %10.5f %10.5f %10.5f %10.5f \n",
t1+i/n2*t2, x2[1,i], x2[2,i], x2[3,i], x2[4,i], u2[i], -1.2+i/n2, p[7]+sqrt(1-(i/n2)^2) >> z;