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 := 0.2;
param alpha{1..V,1..V} := 2;
param denominator; # investigate further sum of all interfering signals and noise

param totalTime := T;
param dT := 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;

minimize Time:
#	0;
	(Tmax
	+ 
	100*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)
	)
; #	+
 #	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 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] >=  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 <= speed[v,t+1] - speed[v,t] <= 0.5;
subject to acc0{v in 1..V}:
	0 <= speed[v,1] <= 0.5;
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}:
	s[v,t] = s[v,t-1] + speed[v,t]*dT;
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;
subject to stoponend{v in 1..V}:
	speed[v,T] <= 1e-4;

data 20vpath.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 iterlim=3000";
#option loqo_options "verbose=2 inftol=1e-5 sigfig=6 iterlim=3000";

solve;
display Communication;
display x, y;
display speed;
display Loss;
display s;

display Arrived;
display Tmax;
display transmit;

display Lack;
display sgloss;


repeat {
	let subtour := 0;
	for {t in 1..T} {
		let Done := {1};
		let ToDo := {2..V};
		for {2..V} {
			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};
					}
					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 Communication;
display x, y;
display speed;
display Loss;
display s;

display Arrived;
display Tmax;
display transmit;

display Lack;
display sgloss;

let x[4,7] := splinex[4,numPoints-1];
let y[4,7] := spliney[4,numPoints-1];
let x[4,8] := splinex[4,numPoints-1];
let y[4,8] := spliney[4,numPoints-1];
let x[4,9] := splinex[4,numPoints-1];
let y[4,9] := spliney[4,numPoints-1];
display allthere;