#include <ildispat/ilodispatcher.h>

ILOSTLBEGIN

void Info(IloDispatcher dispatcher) 
{
  IloSolver solver = dispatcher.getSolver();
  solver.printInformation();
  dispatcher.printInformation();
  solver.out() << "===============" << endl
               << "Cost         : " << dispatcher.getTotalCost() << endl 
               << "Number of vehicles used : " 
               << dispatcher.getNbOfVehiclesUsed() << endl
               << "Solution     : " << endl
              // << dispatcher << endl;
			   << IloTerse(dispatcher) << endl;
}

IloVisitArray OrderVisits(IloEnv env, IloVisitArray visits, IloDistance dist, IloDimension1 tardiness) 
{
  IloModel tspModel(env);
  IloInt nbOfVisits = visits.getSize();
  IloVisit first(visits[0].getNode(), "first");
  IloVisit last(visits[0].getNode(), "last");
  IloVehicle vehicle(first, last, "TSP");
  tspModel.add(vehicle);
  IloDimension2 dim(env, dist, IloFalse);
  vehicle.setCost(dim, 1.0);
//test
  IloDimension1 tard(env, IloFalse, "tardiness");
  vehicle.setCost(tard, 1.0);
  tspModel.add(tard);
  //test end

  tspModel.add(dim);
  IloInt i;
  for (i = 1; i < nbOfVisits; i++)
    tspModel.add(visits[i]);
  IloSolver solver(tspModel);
  IloDispatcher dispatcher(solver);
  IloGoal instCost = IloDichotomize(env, dispatcher.getCostVar(), IloFalse);
  solver.out() << "Producing insertion order" << endl;
  solver.solve(IloNearestAdditionGenerate(env) && instCost);
  IloRoutingSolution rsolution(tspModel);
  rsolution.store(solver);
  IloNHood nhood = IloTwoOpt(env);
  IloMetaHeuristic improve = IloImprove(env);
  IloGoal move = IloSingleMove(env, rsolution, nhood, improve, instCost);
  while (solver.solve(move)) { }
  IloVisitArray orderedVisits(env, nbOfVisits);
  IloRoutingSolution::RouteIterator rit(rsolution, vehicle);
  ++rit;
  orderedVisits[0] = visits[0];
  for (IloInt k = 1; k < nbOfVisits; ++k, ++rit)
    orderedVisits[k] = *rit;
  nhood.end();
  solver.end();
  rsolution.end();
  return orderedVisits;
}

int main(int argc, char* argv[]) {
  IloEnv env;
  try {
    IloModel mdl(env);
    IloDimension2 length(env, IloEuclidean, "Length");
    mdl.add(length);
    IloDimension1 weight(env, "Weight");
    mdl.add(weight);
	//test
	IloDimension1 tardiness(env, IloFalse, "Tardiness");
	mdl.add(tardiness);
	//test end
    ifstream infile;
    char * problem;
    if (argc >= 2) problem = argv[1];
    else           problem = (char *) "../../../examples/data/vrp5.dat";
    infile.open(problem);
    if (!infile) { 
      env.out() << "File not found or not specified: " << problem << endl;
      env.end();
      return 0;
    }
    IloInt nbOfVisits, nbOfTrucks; 
    infile >> nbOfVisits >> nbOfTrucks;
    //nbOfTrucks = 3;
    IloNum capacity;
    infile >> capacity;
    IloNum depotX, depotY, openTime, closeTime;
    infile >> depotX  >> depotY >> openTime >> closeTime;
    IloNode depot(env, depotX, depotY);
    IloInt i;
    for (i = 0; i < nbOfTrucks; i++) {
      IloVisit first(depot, "Depot");
	  mdl.add(first.getCumulVar(length) >= openTime);
      mdl.add(first.getTransitVar(tardiness) == 0);
      IloVisit last(depot, "Depot");
	  mdl.add(last.getCumulVar(length) <= closeTime);
      mdl.add(last.getTransitVar(tardiness) == 0);
	  
      char name[20];
      sprintf(name, "Vehicle %ld", i);
      
	  IloVehicle vehicle(first, last, name);
 //     vehicle.setCost(250.0);
      vehicle.setCost(length, 1.0);
      vehicle.setCapacity(weight, capacity);
 //     last.getCumulVar(length).setUb(350);

	  vehicle.setCost(tardiness, 1.0);
      mdl.add(vehicle);
    }
    IloVisitArray visits(env, nbOfVisits);
    for (i = 0; i < nbOfVisits; i++) {
      IloInt id;
      IloNum x, y, quantity, minTime, maxTime, dropTime;
      infile >> id >> x >> y >> quantity >> minTime >> maxTime >> dropTime;
      IloNode customer(env, x, y);
      char name[10];
      sprintf(name, "%ld", id);
      IloVisit visit(customer, name);
      visit.getTransitVar(weight).setBounds(quantity, quantity);
      visits[i] = visit; 
	  IloNumVar tard(env, 0, closeTime); 
      mdl.add(tard >= visit.getCumulVar(length) - maxTime);
	  mdl.add(tard >= minTime - visit.getCumulVar(length));
	  
      mdl.add(visit.getTransitVar(tardiness) == tard);
	  visit.getDelayVar(length) == dropTime;

    }
    infile.close();
    IloSolver solver(mdl);
    IloDispatcher dispatcher(solver);
    IloRoutingSolution solution(mdl);
    IloGoal instCost = IloDichotomize(env, dispatcher.getCostVar(), IloFalse);
    solver.out() << "Inserting return visits" << endl;
    for (IloVehicleIterator vehIt(mdl); vehIt.ok(); ++vehIt) {
      IloVehicle vehicle = *vehIt;
      for (i = 0; i < 2; i++) {
        IloVisit visit(depot, "depot");
        visit.setPenaltyCost(-0.1);
        mdl.add(visit);
        IloGoal ins = IloInsertVisit(env, visit, vehicle, solution, instCost);
        if (solver.solve(ins)) {
          solution.add(visit);
          solution.store(solver);
        }
        else {
          solver.out() << "Could not insert return visit." << endl;
          mdl.remove(visit);
        }
      }
    }
    IloDistance distance = IloDistance(env, IloEuclidean);
    //IloVisitArray orderedVisits = OrderVisits(env, visits, distance);
	//test
	IloVisitArray orderedVisits = OrderVisits(env, visits, distance, tardiness);
	//test end

    visits.end();
    solver.out() << "Inserting customer visits" << endl;
    for (i = 0; i < nbOfVisits; i++) {
      IloVisit visit = orderedVisits[i];
      mdl.add(visit);
      IloGoal insert = IloInsertVisit(env, visit, solution, instCost);
      if (!solver.solve(insert)) {
        solver.error() << "Could not generate an initial solution" << endl;
        env.end();
        return 0;
      }
      solution.add(visit);
      solution.store(solver);
    }
    IloNHood nhood = IloRelocate(env)
                   + IloTwoOpt(env) 
                   + IloOrOpt(env)
                   + IloCross(env);
    nhood = nhood  + IloExchange(env)
                   + IloMakeUnperformed(env)
                   + IloMakePerformed(env);
    IloMetaHeuristic improve = IloImprove(env);
    IloGoal move = IloSingleMove(env, solution, nhood, improve, instCost);
    solver.out() << "Improving" << endl;
    while (solver.solve(move)) { }
    solver.solve(IloRestoreSolution(env, solution));
    Info(dispatcher);
  } catch(IloException& ex) {
    cerr << "Error: " << ex << endl;
  } 
  env.end();
  return 0;
}