param k := 1;

set Done;
set ToDo;
param subtour;
param nst;

param T;
param V;
param numPoints;

param splinex{1..V,0..numPoints-1};
param spliney{1..V,0..numPoints-1};

param path_slope{1..V};
param threshold;
param transmit := 2.7;
param alpha{1..V,1..V} := 2;
param denominator; # investigate further sum of all interfering signals and noise

param totalTime := T;
param dT := 1/3; #totalTime/T;

param pieceLength{1..V, 0..numPoints-1};
param ax{1..V,1..numPoints-1,1..4};
param ay{1..V,1..numPoints-1,1..4};

param Coeff{1..V,1..V,1..T,1..nst};

var speed{v in 1..V,1..T} <= 2, >= 0, := 1;
var s{v in 1..V, t in 0..T};
var x{v in 1..V,t in 0..T} := Uniform01();
var y{v in 1..V,t in 0..T} := Uniform01();

var dist{v1 in 1..V, v2 in 1..V, t in 0..T: v1 != v2} = sqrt( (x[v1,t]-x[v2,t])^2 + (y[v1,t]-y[v2,t])^2 );
var Communication{v1 in 1..V, v2 in 1..V, t in 0..T: v1 != v2} >= 0, <= 1, := 1;
var Loss{v1 in 1..V, v2 in 1..V, t in 0..T: v1 != v2} = transmit - threshold*dist[v1,v2,t]^alpha[v1,v2]*denominator;

var Arrived{v in 1..V, t in 0..T} := 0.5, >= 0, <= 1;
var Tmax >= 0, := 1;

var Lack{v in 1..V, t in 0..T} := 0.5, <= 0;
var sgloss >= 0 := 0.5;

var allthere{t in 1..T} >= 0, := 0.5, <= 1;

param jV;
param jamx{1..jV, 1..T};
param jamy{1..jV, 1..T};
var pLoss{1..V,1..T} >= 0, := 0;
var cLoss{1..V,0..T} >= 0, := 0;
var partLoss{1..nst} >= 0, := 0;

minimize Time:
#	0;
	10*Tmax
	+ 
	sum {v in 1..V, t in 0..T} ((x[v,t] - splinex[v,numPoints-1])^2 + (y[v,t] - spliney[v,numPoints-1])^2)
#	sum {v in 1..V, t in 0..T} (pieceLength[v,numPoints-1] - s[v,t])^2
#	+ 
#	1000*sum {v in 1..V, t in 2..T-1} cLoss[v,t]^2
#	+
#	sum {st in 1..nst} partLoss[st]^2
; #	+
 #	1000*sum{v in 1..V, t in 0..T} Lack[v,t]
 #	+ 10000*sgloss;
subject to Arrival{v in 1..V, t in 0..T}:
        Arrived[v,t]*( (x[v,t] - splinex[v,numPoints-1])^2 + (y[v,t] - spliney[v,numPoints-1])^2) = 0;
subject to TmaxDef{v in 1..V}:
	sum {t in 1..T} (1 - Arrived[v,t]) <= Tmax;
subject to EquationOfMotionx{v in 1..V, t in 1..T}:
	x[v,t] = 
	if ( s[v,t] < pieceLength[v,1] )  then
		ax[v,1,1]*s[v,t]^3 + ax[v,1,2]*s[v,t]^2 + ax[v,1,3]*s[v,t] + ax[v,1,4]
else	if ( pieceLength[v,1] <= s[v,t] < pieceLength[v,2] )  then
		ax[v,2,1]*(s[v,t]-pieceLength[v,1])^3 + ax[v,2,2]*(s[v,t]-pieceLength[v,1])^2 + ax[v,2,3]*(s[v,t]-pieceLength[v,1]) + ax[v,2,4]
else	if ( pieceLength[v,2] <= s[v,t] < pieceLength[v,3] )  then
		ax[v,3,1]*(s[v,t]-pieceLength[v,2])^3 + ax[v,3,2]*(s[v,t]-pieceLength[v,2])^2 + ax[v,3,3]*(s[v,t]-pieceLength[v,2]) + ax[v,3,4]
else
		ax[v,4,1]*(s[v,t]-pieceLength[v,3])^3 + ax[v,4,2]*(s[v,t]-pieceLength[v,3])^2 + ax[v,4,3]*(s[v,t]-pieceLength[v,3]) + ax[v,4,4];
subject to EquationOfMotiony{v in 1..V, t in 1..T}:
	y[v,t] = 
	if ( s[v,t] < pieceLength[v,1] )  then
		ay[v,1,1]*s[v,t]^3 + ay[v,1,2]*s[v,t]^2 + ay[v,1,3]*s[v,t] + ay[v,1,4]
else	if ( pieceLength[v,1] <= s[v,t] < pieceLength[v,2] )  then
		ay[v,2,1]*(s[v,t]-pieceLength[v,1])^3 + ay[v,2,2]*(s[v,t]-pieceLength[v,1])^2 + ay[v,2,3]*(s[v,t]-pieceLength[v,1]) + ay[v,2,4]
else	if ( pieceLength[v,2] <= s[v,t] < pieceLength[v,3] )  then
		ay[v,3,1]*(s[v,t]-pieceLength[v,2])^3 + ay[v,3,2]*(s[v,t]-pieceLength[v,2])^2 + ay[v,3,3]*(s[v,t]-pieceLength[v,2]) + ay[v,3,4]
else	
		ay[v,4,1]*(s[v,t]-pieceLength[v,3])^3 + ay[v,4,2]*(s[v,t]-pieceLength[v,3])^2 + ay[v,4,3]*(s[v,t]-pieceLength[v,3]) + ay[v,4,4];
#subject to jammed{v1 in 1..V, v2 in 1..jV-1, t in 2..T-1}:
#	(x[v1,t] - jamx[v2,t])^2 + (y[v1,t] - jamy[v2,t])^2 >= 0.6*(1-Arrived[v1,t]); #+ pLoss[v1,t] >= 0.4;
#subject to Comm_jam{v1 in 1..V, v2 in 1..V, t in 2..T-1: v1 != v2}:
#	Communication[v1,v2,t]*(pLoss[v1,t]+pLoss[v2,t]) <= 0;
subject to Comm_equil{v1 in 1..V, v2 in 1..V, t in 0..T: v1 != v2}:
	Communication[v1,v2,t]*Loss[v1,v2,t] >= 0;
subject to In_Communication{v1 in 1..V, t in 0..T}:
	sum {v2 in 1..V: v1 != v2} Communication[v1,v2,t] + cLoss[v1,t] >=  k;
subject to scons{v in 1..V, t in 1..T}:
	0 <= s[v,t] <= pieceLength[v,numPoints-1];
subject to acc{v in 1..V, t in 1..T-1}:
	-1*dT <= (speed[v,t+1] - speed[v,t]) <= 0.5*dT;
subject to acc0{v in 1..V}:
	0 <= speed[v,1] <= 0.5*dT;
#subject to safe{v1 in 1..V, v2 in 1..V, t in 1..T: v1 != v2}:
#	(x[v1,t] - x[v2,t])^2 + (y[v1,t] - y[v2,t])^2 >= 1e-4;
subject to arcDef{v in 1..V, t in 1..T}:
	speed[v,t]*dT = (s[v,t] - s[v,t-1]);
#subject to allthereDef{t in 1..T}:
#	(1 - allthere[t])*(Tmax - t) <= 0;
subject to subtour_elim{st in 1..nst}:
	sum {v1 in 1..V, v2 in 1..V, t in 1..T: v1 != v2} Coeff[v1,v2,t,st]*Communication[v1,v2,t] >= 1; #+ partLoss[st] >= 1;
#subject to stoponend{v in 1..V}:
#	speed[v,T] <= 1e-4;

data 4vpath.dat;
let nst := 0;
fix {v in 1..V} x[v,0] := splinex[v,0];	
fix {v in 1..V} y[v,0] := spliney[v,0];	
fix {v in 1..V} x[v,T] := splinex[v,numPoints-1];	
fix {v in 1..V} y[v,T] := spliney[v,numPoints-1];	
fix {v in 1..V} s[v,0] := pieceLength[v,0];
fix {v in 1..V} s[v,T] := pieceLength[v,numPoints-1];

option presolve_eps 1e-4;
#option loqo_options "verbose=0 iterlim=3000";
option loqo_options "verbose=2 sigfig=6 iterlim=3000";
#option loqo_options "verbose=2 inftol=1e-5 sigfig=6 iterlim=3000";

solve;
display {v1 in 1..V, v2 in 1..1, t in 2..T-1}: (x[v1,t] - jamx[v2,t])^2 + (y[v1,t] - jamy[v2,t])^2;
repeat {
display partLoss;
display pLoss;
display cLoss;
display jamx, jamy;
display Communication;
display x, y;
display speed;
display Loss;
display s;
display Arrived;
display Tmax;
display transmit;
        let subtour := 0;
        for {t in 1..Tmax} {
                let Done := {1};
printf "Done includes 1\n";
                let ToDo := {2..V};
                for {v in 2..V} {
		#	if ( cLoss[v,t] > 1e-3 ) then {
		#		let Done := Done union {v};
		#		let ToDo := ToDo diff {v};
#printf "Done includes %d\n", v;
		#	} else {
				if !( ToDo within {} ) then {
					for {v1 in Done, v2 in ToDo} {
						if ( Communication[v1,v2,t] > 1e-4 ) then {
							let Done := Done union {v2};
							let ToDo := ToDo diff {v2};
printf "Done includes %d\n", v2;
						}
						if ( ToDo within {} ) then break;
					}
				}
			}
#                }
                if !( ToDo within {} ) then {
printf "Adding subtour at t = %d\n", t;
                        let nst := nst + 1;
                        let {v1 in 1..V, v2 in 1..V, tt in 1..T} Coeff[v1,v2,tt,nst] := 0;
                        let {v1 in Done, v2 in ToDo} Coeff[v1,v2,t,nst] := 1;
                        let subtour := 1;
                }
        }
        if ( subtour = 1 )
                then solve;
                else printf "No additional subtours found.\n";
}
while subtour = 1;

display partLoss;
display pLoss;
display cLoss;
display Communication;
display x, y;
display speed;
display Loss;
display s;

display Arrived;
display Tmax;
display transmit;

display Lack;
display sgloss;

display jamx, jamy;