load('lecture12-data.mat');

%% Plot the instance
figure;
scatter(depot_coordinates(1,1), depot_coordinates(1,2), 'g', 'filled');
text(depot_coordinates(1,1), depot_coordinates(1,2), "  Depot");
hold on
for i = 1:size(customer_coordinates,1)
    scatter(customer_coordinates(i,1), customer_coordinates(i,2), 'k', 'filled');
    text(customer_coordinates(i,1), customer_coordinates(i,2), "  " + num2str(i));
end
hold off
xlim([-1,3]);
ylim([-1,3]);

%% Exercise a)

coordinates = [depot_coordinates; customer_coordinates];
[tour, tourlength] = nearestNeighborTSP(coordinates);

tsp_cost = tourlength*4 + size(customer_coordinates,1)*1;

hold on
for i = 1:size(tour,2)-1
    x = coordinates(tour(i),:);
    y = coordinates(tour(i+1),:);
    plot([x(1), y(1)], [x(2),y(2)], 'k')
end
hold off


%% Exercise b
distances = [];
for i = 1:size(customer_coordinates,1)
    distances(i) = sum(sqrt(sum((customer_coordinates - customer_coordinates(i, :)).^2, 2)));
end
central_depot = find(distances == min(distances));

%% Exercise c
cost = [];
SL = [];
for i = 1:5
    linehaul =  sqrt(sum((depot_coordinates - customer_coordinates(i, :)).^2, 2));
    unserved = max(demand-A(i,:)',0);
    served = demand -unserved;
    cost(i) = 4*linehaul + 1 + 5 + 1*sum(served) + 3*sum(unserved);
    SL(i) = sum(served)/sum(demand);
end

best_depot = find(cost == min(cost));

%% Exercise d 

fprintf('The best node performs %.2f %% better than the central node\n', (1 - cost(best_depot)/cost(central_depot))*100);


%% Exercise e

coordinates = [depot_coordinates; customer_coordinates];
[tour, tourlength] = nearestNeighborTSP(coordinates);

tsp_cost_new = tourlength*2 + size(customer_coordinates,1)*1;


function [tour, tourlength] = nearestNeighborTSP(coordinates)
numCities = size(coordinates, 1);
unvisited = 1:numCities;
tour = zeros(1, numCities);
tourlength = 0;

% Start from the first city
currentCity = 1;
tour(1) = currentCity;
unvisited(unvisited == currentCity) = [];

% Find nearest neighbors iteratively
for i = 2:numCities
    distances = sqrt(sum((coordinates(unvisited, :) - coordinates(currentCity, :)).^2, 2));
    [mindist, index] = min(distances);
    currentCity = unvisited(index);
    tour(i) = currentCity;
    tourlength = tourlength + mindist;
    unvisited(unvisited == currentCity) = [];
end

% Return to the starting city to complete the tour
tour = [tour, tour(1)];
tourlength = tourlength + sqrt(sum((coordinates(tour(numCities), :) - coordinates(tour(1), :)).^2, 2));
end

