###############################################################################
# external parameters                                                         #
###############################################################################

param M;
param dt;
param a_max;

param nb_pts, integer;
param x_ref{i in 1..nb_pts};
param y_ref{i in 1..nb_pts};

set CartEqn;
param nb_obs, integer;
param obs{i in 1..nb_obs, j in 1..4, k in CartEqn};

display obs;

###############################################################################
# variables                                                                   #
###############################################################################

var x{i in 1..nb_pts};
var y{i in 1..nb_pts};
var v_x{i in 1..nb_pts};
var v_y{i in 1..nb_pts};
var a_x{i in 1..nb_pts};
var a_y{i in 1..nb_pts};

var x_err{i in 1..nb_pts};
var y_err{i in 1..nb_pts};

var b{i in 1..nb_pts,j in 1..nb_obs,k in 1..4}, binary;


###############################################################################
# objective                                                                   #
###############################################################################

minimize f : sum {i in 1..nb_pts} (x_err[i] + y_err[i]);


###############################################################################
# constraints                                                                 #
###############################################################################

s.t. c00_{i in 1..nb_pts-1} :   x[i+1] = x[i] + v_x[i] * dt;
s.t. c01_{i in 1..nb_pts-1} :   y[i+1] = y[i] + v_y[i] * dt;

s.t. c02_{i in 1..nb_pts-1} :   v_x[i+1] = v_x[i] + a_x[i] * dt;
s.t. c03_{i in 1..nb_pts-1} :   v_y[i+1] = v_y[i] + a_y[i] * dt;

s.t. c04_{i in 1..nb_pts}   :    - a_max  <= a_x[i] <= a_max;
s.t. c05_{i in 1..nb_pts}   :    - a_max  <= a_y[i] <= a_max;

s.t. c06_{i in 1..nb_pts}   :   x[i] - x_ref[i] <= x_err[i];
s.t. c07_{i in 1..nb_pts}   :  -x[i] + x_ref[i] <= x_err[i];

s.t. c08_{i in 1..nb_pts}   :   y[i] - y_ref[i] <= y_err[i];
s.t. c09_{i in 1..nb_pts}   :  -y[i] + y_ref[i] <= y_err[i];

# constraint is active for b[i,j,k] = 0 : (x,y)[i] is outside

s.t. c10_{i in 1..nb_pts, j in 1..nb_obs, k in 1..4} : obs[j,k,'a'] * x[i] + obs[j,k,'b'] * y[i] >= - obs[j,k,'c'] - M *      b[i,j,k];

# constraint is active for b[i,j,k] = 1 : (x,y)[i] is inside

s.t. c11_{i in 1..nb_pts, j in 1..nb_obs, k in 1..4} : obs[j,k,'a'] * x[i] + obs[j,k,'b'] * y[i] <= - obs[j,k,'c'] + M * (1 - b[i,j,k]);

# the points should be outside of each obstacle

s.t. c12_{i in 1..nb_pts, j in 1..nb_obs} : sum{k in 1..4} b[i,j,k] <= 3;

###############################################################################
# resolution                                                                  #
###############################################################################

solve;


###############################################################################
# text output                                                                 #
###############################################################################

param txt_filename, symbolic;

printf   "x\ty\tv_x\tv_y\ta_x\ta_y\tx_err\ty_err\n"                      > txt_filename;
printf{i in 1..nb_pts} 
         "%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n",
         x[i], y[i], v_x[i], v_y[i], a_x[i], a_y[i], x_err[i], y_err[i] >> txt_filename;



###############################################################################
# svg output                                                                  #
###############################################################################

param svg_filename, symbolic;
param svg_scale;

printf "<?xml version=""1.0"" standalone=""no""?>\n"               > svg_filename;
printf "<!DOCTYPE svg PUBLIC ""-//W3C//DTD SVG 1.1//EN"" "        >> svg_filename;
printf """http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd"">\n"  >> svg_filename;
printf "<svg width=""100\%"" height=""100\%"" version=""1.0"" "   >> svg_filename;
printf "xmlns=""http://www.w3.org/2000/svg"">\n"                  >> svg_filename;

# draw red points for reference
for {i in 1..nb_pts}
  {
  printf "<circle cx=""%f"" cy=""%f"" r=""2"" ",
         svg_scale * x_ref[i], 
         svg_scale * y_ref[i] 
         >> svg_filename;
  printf "style=""stroke:red;fill:red""/>\n"
         >> svg_filename;
  }

# draw dashed red lines for reference
for {i in 1..nb_pts-1}
  {
  printf "<line x1=""%f"" y1=""%f"" x2=""%f"" y2=""%f"" ",
         svg_scale * x_ref[i], 
         svg_scale * y_ref[i], 
         svg_scale * x_ref[i+1], 
         svg_scale * y_ref[i+1] 
         >> svg_filename;
  printf "style=""stroke:red;stroke-dasharra_y: 3 3""/>\n"
         >> svg_filename;
  }

# draw obstacles
for {j in 1..nb_obs}
  {
  printf "<polygon points=""%f,%f %f,%f %f,%f %f,%f"" ",
         svg_scale * (obs[j,1,'b'] * obs[j,2,'c'] - obs[j,1,'c'] * obs[j,2,'b']) / (obs[j,1,'a'] * obs[j,2,'b'] - obs[j,1,'b'] * obs[j,2,'a']),
         svg_scale * (obs[j,1,'c'] * obs[j,2,'a'] - obs[j,1,'a'] * obs[j,2,'c']) / (obs[j,1,'a'] * obs[j,2,'b'] - obs[j,1,'b'] * obs[j,2,'a']),
         svg_scale * (obs[j,2,'b'] * obs[j,3,'c'] - obs[j,2,'c'] * obs[j,3,'b']) / (obs[j,2,'a'] * obs[j,3,'b'] - obs[j,2,'b'] * obs[j,3,'a']),
         svg_scale * (obs[j,2,'c'] * obs[j,3,'a'] - obs[j,2,'a'] * obs[j,3,'c']) / (obs[j,2,'a'] * obs[j,3,'b'] - obs[j,2,'b'] * obs[j,3,'a']),
         svg_scale * (obs[j,3,'b'] * obs[j,4,'c'] - obs[j,3,'c'] * obs[j,4,'b']) / (obs[j,3,'a'] * obs[j,4,'b'] - obs[j,3,'b'] * obs[j,4,'a']),
         svg_scale * (obs[j,3,'c'] * obs[j,4,'a'] - obs[j,3,'a'] * obs[j,4,'c']) / (obs[j,3,'a'] * obs[j,4,'b'] - obs[j,3,'b'] * obs[j,4,'a']),
         svg_scale * (obs[j,4,'b'] * obs[j,1,'c'] - obs[j,4,'c'] * obs[j,1,'b']) / (obs[j,4,'a'] * obs[j,1,'b'] - obs[j,4,'b'] * obs[j,1,'a']),
         svg_scale * (obs[j,4,'c'] * obs[j,1,'a'] - obs[j,4,'a'] * obs[j,1,'c']) / (obs[j,4,'a'] * obs[j,1,'b'] - obs[j,4,'b'] * obs[j,1,'a'])
         >> svg_filename;
  printf "style=""fill:blue;stroke:blue;stroke-width:2;fill-opacity:0.6;stroke-opacity:0.6""/>\n"
         >> svg_filename;
  }

# draw black points for solution
for {i in 1..nb_pts}
  {
  printf "<circle cx=""%f"" cy=""%f"" r=""2"" ",
         svg_scale * x[i], 
         svg_scale * y[i] 
         >> svg_filename;
  printf "style=""stroke:black""/>\n"
         >> svg_filename;
  }

# draw solid black lines for solution
for {i in 1..nb_pts-1}
  {
  printf "<line x1=""%f"" y1=""%f"" x2=""%f"" y2=""%f"" ",
         svg_scale * x[i], 
         svg_scale * y[i], 
         svg_scale * x[i+1], 
         svg_scale * y[i+1] 
         >> svg_filename;
  printf "style=""stroke:black;stroke-width:2""/>\n"
         >> svg_filename;
  }

printf "</svg>\n" >> svg_filename;



end;