param pi := 4*atan(1);
param g := 9.8e+2;  # acc. due to gravity

param tau_bnd := 250;

param I1 := 1;
param I2 := 1;
param l1 := 1;
param l2 := 1;
param lc1 := l1/2;
param lc2 := l2/2;
param m1 := 1;
param m2 := 1;

param n;
param dt := 1/n;

var q1 {j in 0..n} := -pi/2 + pi*j*dt;
var q2 {j in 0..n} := 0;

var q1dot {j in 0..n by 1/2};
var q2dot {j in 0..n by 1/2};

var q1dot2 {j in 0..n};
var q2dot2 {j in 0..n};

var   d11 {j in 0..n} = m1*lc1^2 + m2*(l1^2 + lc2^2 + 2*l1*lc2*cos(q2[j])) + I1 + I2;
param d22 {j in 0..n} = m2*lc2^2 + I2;
var   d12 {j in 0..n} = m2*(lc2^2 + l1*lc2*cos(q2[j])) + I2;
var   d21 {j in 0..n} = m2*(lc2^2 + l1*lc2*cos(q2[j])) + I2;

var h1 {j in 0..n} =   -m2*l1*lc2*sin(q2[j])*q2dot[j]^2
                    - 2*m2*l1*lc2*sin(q2[j])*q2dot[j]
					    *q1dot[j];
var h2 {j in 0..n} =    m2*l1*lc2*sin(q2[j])*q1dot[j]^2;

var phi1 {j in 0..n} = (m1*lc1 + m2*l1)*g*cos(q1[j]) + m2*lc2*g*cos(q1[j] + q2[j]);
var phi2 {j in 0..n} =                                 m2*lc2*g*cos(q1[j] + q2[j]);

var tau {j in 0..n} >= -tau_bnd, <= tau_bnd; 
var abstau {j in 1..n-1};

#minimize total_torque_squared: sum {j in 1..n-1} sqrt(1e-4 + tau[j]^2) * dt;
#minimize total_torque_squared: sum {j in 1..n-1} tau[j]^2 * dt;
#minimize total_torque_squared: sum {j in 1..n-1} (q1dot2[j]^2 + q2dot2[j]^2)* dt;
#minimize zero: 0;
minimize taubound: (2/dt) * sum {j in 1..n-1} abstau[j]*dt;

subject to upbnd {j in 1..n-1}: tau[j] <= abstau[j];
subject to lobnd {j in 1..n-1}: -abstau[j] <= tau[j];

subject to    q1_init:     q1[0] = -pi/2;
subject to    q2_init:     q2[0] =  0;
subject to q1dot_init:  q1dot[0] =  0;
subject to q2dot_init:  q2dot[0] =  0;
subject to    q1_final:    q1[n] =  pi/2;
subject to    q2_final:    q2[n] =  0;
subject to q1dot_final: q1dot[n] =  0;
subject to q2dot_final: q2dot[n] =  0;

subject to q1def {j in (1/2)..(n-1/2)}: (q1[j+1/2]-q1[j-1/2])/dt = q1dot[j];
subject to q2def {j in (1/2)..(n-1/2)}: (q2[j+1/2]-q2[j-1/2])/dt = q2dot[j];

subject to q1dotdef1 {j in (1/2)..(n-1/2)}: (q1dot[j]-q1dot[j-1/2])/(dt/2) = q1dot2[j-1/2];
subject to q2dotdef1 {j in (1/2)..(n-1/2)}: (q2dot[j]-q2dot[j-1/2])/(dt/2) = q2dot2[j-1/2];

subject to q1dotdef2 {j in (1/2)..(n-1/2)}: (q1dot[j+1/2]-q1dot[j])/(dt/2) = q1dot2[j+1/2];
subject to q2dotdef2 {j in (1/2)..(n-1/2)}: (q2dot[j+1/2]-q2dot[j])/(dt/2) = q2dot2[j+1/2];

subject to Newton1 {j in 0..n}: 
	d11[j]*q1dot2[j] + d12[j]*q2dot2[j] + h1[j] + phi1[j] = 0;
subject to Newton2 {j in 0..n}: 
	d21[j]*q1dot2[j] + d22[j]*q2dot2[j] + h2[j] + phi2[j] = tau[j];


#subject to smoothness1 {j in 1..n-1}: -50 <= q1dot2[j] <= 50;
#subject to smoothness2 {j in 1..n-1}: -50 <= q2dot2[j] <= 50;
#subject to smoothness {j in 1..n-1}: 
#    -30 <= (tau[j-1]-2*tau[j]+tau[j+1])/dt^2 <= 30;
#subject to smoothness_lo {j in   0..50}: -0.01 <= tau[j] <= 0.01;
#subject to smoothness_hi {j in n-50..n}: -0.01 <= tau[j] <= 0.01;
#fix {j in   0..1} tau[j] := 0;
#fix {j in n-1..n} tau[j] := 0;
#fix {j in   3..n-3} tau[j] := 0;

#option loqo_options "verbose=2 inftol=1e-1 iterlim=50";
#option loqo_options "verbose=2 inftol=4 epsdiag=1e-5";
option loqo_options "verbose=2 sigfig=15 inftol=2e-3";
let n := 5000;
#option solver snopt;
solve;

printf {j in 0..n}: "%10f %10f \n", l1*cos(q1[j]), l1*sin(q1[j]) > "xy1.txt";
printf {j in 0..n}: "%10f %10f \n", 
       l1*cos(q1[j])+l2*cos(q1[j]+q2[j]), l1*sin(q1[j])+l2*sin(q1[j]+q2[j]) > "xy2.txt";
printf {j in 0..n}: "%10f \n", tau[j] > "tau.txt";
printf {j in 0..n}: "tau[%5d] = %10f \n", j, tau[j] > "tau_of_j.txt";

display max{j in 1..n-1} tau[j];
display min{j in 1..n-1} tau[j];

printf {j in 0..5}: 
  "qdot[1] = %16.12f, qdot[2] = %16.12f \n qdot2[1] = %16.12f, qdot2[2] = %16.12f \n", 
  q1dot[j+1/2],  q2dot[j+1/2], q1dot2[j], q2dot2[j];