DE_optomization.m

%Content is user-generated and unverified.
% Enhanced Differential Evolution for Path Planning
% Incorporates robust strategies from DE_50.m while maintaining example3_DE structure

% Initialize GA-FuL library .NET assemblies
gafulInit;

% Generate maze
obstacles = polygonMaze(1, 6, 12, 15);

% Solve the maze using Voronoi diagram
mazeSolver = solveMazeViaVoronoi(obstacles, 1, 100, 0.15, 0.5);
pathFinder = mazeSolver.AdjacencyGraph.GetAStarPathFinder();

% Find the shortest path
voronoiPath = pathFinder.FindPath(0, 0, -10, 0.5, obstacles, 0.2, 1e-2);
voronoiPathMetrics = voronoiPath.GetMetrics(obstacles);

% Create\load pre-computed chache to accelerate PH curve computations.
% This is only needed once before computing PH poly-curves.
phCache = phc2cache(5);

% Construct a full PH poly-curve for path
polyCurve = phPolyCurve(voronoiPath, phCache, obstacles, 0.2);

% Set physical scaling to deduce physical properties of path
% Each unit of curve length is equal to 1000 meters distance
% Assume UAV is flying at constant speed of 20 m/s
polyCurve.SetPhysicalProperties(1000, 20);

% These characteristics can be used to optimize the resulting poly curve:
% All quantities should be minimized, we may use multi-objective optimize
% on all/some of them

% Physical total length of curve in meters:
totalLength = polyCurve.PhysicalTotalLength;
disp2(totalLength, 'Physical Total Length')

% Physical Curvature Range in 1/meters (a structure with 3 members: Min, Max, Mean)
curvatureRange = polyCurve.GetPhysicalCurvatureRange(0.1, 5);
disp2(curvatureRange, 'Physical Curvature Range')

% Physical Lateral Acceleration Range in meters/sec. (a structure with 3 members: Min, Max, Mean)
lateralAccRange = polyCurve.GetPhysicalLateralAccelerationRange(0.1, 5);
disp2(lateralAccRange, 'Physical Lateral Acceleration Range')

% Physical Bank Angle Range in radians (a structure with 3 members: Min, Max, Mean)
bankAngleRange = polyCurve.GetPhysicalBankAngleRange(0.1, 5);
disp2(bankAngleRange, 'Physical Bank Angle Range')

% Plot maze with Voronoi path
fig = plotMaze(obstacles, 'b-', 1);
%plotSegments(voronoiPath, 'r-', 1);
plotSegments(polyCurve, 'r-', 1);

voronoiPath.ToString()
voronoiPathMetrics.ToString()

%% Enhanced Differential Evolution Configuration
config = struct();
config.geneSize = 2 * (voronoiPath.Count - 2);
config.populationSize = 50;           % NP = 50 (typical: 5-10 * D)
config.maxIterations = 2000;

% DE Strategy: DE/rand-to-best/1/bin with adaptive parameters
config.strategy = 'rand-to-best';     % Balances exploration and exploitation
config.F_base = 0.5;                  % Base scaling factor
config.F_min = 0.4;                   % Minimum F for adaptation
config.F_max = 0.9;                   % Maximum F for adaptation
config.CR_base = 0.9;                 % Base crossover rate
config.CR_min = 0.7;                  % Minimum CR
config.CR_max = 0.95;                 % Maximum CR
config.adaptiveDE = true;             % Enable jDE-like adaptation
config.tau1 = 0.1;                    % Probability to adjust F
config.tau2 = 0.1;                    % Probability to adjust CR

% Path planning specific parameters
config.minSafetyDistance = 0.1;
config.pathSafetyDistance = 0.1;
config.convergenceThreshold = 1e-4;
config.convergenceWindow = 20;

% Multi-objective weights
config.weights.angle = 0.5;
config.weights.length = 0.3;
config.weights.clearance = 0.2;

% Diversity management
config.diversityThreshold = 0.05;
config.perturbationRate = 0.15;

%% Initialize Population with Latin Hypercube Sampling
fprintf('Initializing population with LHS...\n');
population = lhsInitialization(config.populationSize, config.geneSize);

% Seed best individual with Voronoi solution
try
    voronoiGene = extractVoronoiGene(voronoiPath);
    population(1, :) = voronoiGene;
    % Add perturbed Voronoi solutions
    for i = 2:min(5, config.populationSize)
        population(i, :) = voronoiGene + 0.05 * randn(1, config.geneSize);
        population(i, :) = max(0, min(1, population(i, :)));
    end
    fprintf('Voronoi solution seeded successfully\n');
catch
    fprintf('Continuing with LHS initialization\n');
end

%% Initialize Fitness and Feasibility
fitness = inf(config.populationSize, 1);
feasible = false(config.populationSize, 1);

fprintf('Evaluating initial population...\n');
for i = 1:config.populationSize
    [fitness(i), feasible(i)] = evaluateFitnessAdvanced(...
        population(i, :), voronoiPath, obstacles, config);
end

% Find initial best
[bestFitness, bestIdx] = min(fitness);
bestSolution = population(bestIdx, :);
bestFeasible = feasible(bestIdx);

% If no feasible solution, find best infeasible
if ~bestFeasible
    infeasibleFitness = fitness(~feasible);
    if ~isempty(infeasibleFitness)
        [bestFitness, relIdx] = min(infeasibleFitness);
        infeasibleIndices = find(~feasible);
        bestIdx = infeasibleIndices(relIdx);
        bestSolution = population(bestIdx, :);
    end
end

fprintf('Initial best fitness: %.4f (Feasible: %d)\n', bestFitness, bestFeasible);

%% Initialize Adaptive Parameters
F = config.F_base * ones(config.populationSize, 1);
CR = config.CR_base * ones(config.populationSize, 1);

%% Differential Evolution Main Loop
fprintf('\nStarting enhanced DE optimization...\n');
fitnessHistory = zeros(config.maxIterations, 1);
convergenceCounter = 0;
convergedAt = config.maxIterations;
noImprovementCount = 0;

for iter = 1:config.maxIterations
    
    % Calculate population diversity
    diversity = calculatePopulationDiversity(population);
    
    % Diversity-based perturbation
    if diversity < config.diversityThreshold && iter > 20
        noImprovementCount = noImprovementCount + 1;
        if noImprovementCount > 15
            % Apply diversity injection
            numPerturb = floor(0.3 * config.populationSize);
            worstIndices = getWorstIndices(fitness, feasible, numPerturb);
            for idx = worstIndices'
                population(idx, :) = bestSolution + ...
                    config.perturbationRate * (rand(1, config.geneSize) - 0.5);
                population(idx, :) = max(0, min(1, population(idx, :)));
                [fitness(idx), feasible(idx)] = evaluateFitnessAdvanced(...
                    population(idx, :), voronoiPath, obstacles, config);
            end
            fprintf('  Diversity injection at iteration %d\n', iter);
            noImprovementCount = 0;
        end
    else
        noImprovementCount = 0;
    end
    
    % Create trial population
    trialPopulation = zeros(config.populationSize, config.geneSize);
    
    for i = 1:config.populationSize
        
        % Adaptive parameter control (jDE-like)
        if config.adaptiveDE
            if rand < config.tau1
                F(i) = config.F_min + rand * (config.F_max - config.F_min);
            end
            if rand < config.tau2
                CR(i) = config.CR_min + rand * (config.CR_max - config.CR_min);
            end
        end
        
        % Mutation strategy: DE/rand-to-best/1
        indices = selectRandomIndices(i, config.populationSize, 2);
        r1 = indices(1);
        r2 = indices(2);
        
        % Get best individual index
        if bestFeasible
            bestIdx = find(fitness == bestFitness & feasible, 1);
        else
            [~, bestIdx] = min(fitness);
        end
        
        % Mutation: V = X_i + F * (X_best - X_i) + F * (X_r1 - X_r2)
        mutantVector = population(i, :) + ...
                      F(i) * (population(bestIdx, :) - population(i, :)) + ...
                      F(i) * (population(r1, :) - population(r2, :));
        
        % Boundary constraint handling using reflection
        mutantVector = boundaryHandling(mutantVector);
        
        % Crossover: Binomial crossover
        jrand = randi(config.geneSize);
        crossoverMask = rand(1, config.geneSize) <= CR(i);
        crossoverMask(jrand) = true;
        
        trialPopulation(i, :) = population(i, :);
        trialPopulation(i, crossoverMask) = mutantVector(crossoverMask);
        
        % Additional boundary check
        trialPopulation(i, :) = max(0, min(1, trialPopulation(i, :)));
    end
    
    % Selection: Evaluate and select better solutions
    for i = 1:config.populationSize
        [trialFitness, trialFeasible] = evaluateFitnessAdvanced(...
            trialPopulation(i, :), voronoiPath, obstacles, config);
        
        % Selection rules with feasibility consideration
        acceptTrial = false;
        
        if trialFeasible && feasible(i)
            acceptTrial = (trialFitness < fitness(i));
        elseif trialFeasible && ~feasible(i)
            acceptTrial = true;
        elseif ~trialFeasible && ~feasible(i)
            acceptTrial = (trialFitness < fitness(i));
        end
        
        if acceptTrial
            population(i, :) = trialPopulation(i, :);
            fitness(i) = trialFitness;
            feasible(i) = trialFeasible;
            
            % Update global best
            if (trialFeasible && trialFitness < bestFitness) || ...
               (trialFeasible && ~bestFeasible)
                bestFitness = trialFitness;
                bestSolution = trialPopulation(i, :);
                bestFeasible = trialFeasible;
                noImprovementCount = 0;
            end
        end
    end
    
    fitnessHistory(iter) = bestFitness;
    
    % Convergence check
    if iter > config.convergenceWindow
        recentChange = abs(fitnessHistory(iter) - ...
            fitnessHistory(iter - config.convergenceWindow));
        
        if recentChange < config.convergenceThreshold && bestFeasible
            convergenceCounter = convergenceCounter + 1;
            if convergenceCounter >= 3
                convergedAt = iter;
                fprintf('  Converged at iteration %d\n', iter);
                break;
            end
        else
            convergenceCounter = 0;
        end
    end
    
    % Display progress every 20 iterations
    if mod(iter, 20) == 0
        fprintf('Iteration %d/%d: Best fitness = %.4f, Feasible = %d, Diversity = %.4f\n', ...
                iter, config.maxIterations, bestFitness, bestFeasible, diversity);
    end
end

fprintf('\nDE optimization complete!\n');
fprintf('Final best fitness: %.4f\n', bestFitness);
fprintf('Converged at iteration: %d\n', convergedAt);

%% Generate and Display Best Path
if bestFeasible
    bestPath = voronoiPath.CreateFromGene(bestSolution, obstacles, config.pathSafetyDistance);
    bestPathMetrics = bestPath.GetMetrics(obstacles);
    
    polyCurve = phPolyCurve(bestPath, phCache, obstacles, 0.2);
    % Plot maze with best found path
        %figure('Name', 'Best Path Found by Enhanced DE', 'NumberTitle', 'off');
    plotMaze(obstacles, 'b-', 1);
    plotSegments(polyCurve, 'r-', 1);
        title(sprintf('Best Path Found by DE (Max Angle: %.2f degrees)', ...
                  bestPathMetrics.GetMaxVertexAngleDegrees()));
    
    % Display the best found path metrics
    fprintf('\n=== Best Path Metrics ===\n');
    disp2(bestPathMetrics);
    bestPathMetrics.ToString()
    
    %% Comparison with Voronoi Path
    fprintf('\n=== Comparison ===\n');
    fprintf('Voronoi Path Max Angle: %.2f degrees\n', ...
            voronoiPathMetrics.GetMaxVertexAngleDegrees());
    fprintf('Voronoi Path Length: %.2f\n', ...
            double(voronoiPathMetrics.Length));
    fprintf('DE Path Max Angle: %.2f degrees\n', ...
            bestPathMetrics.GetMaxVertexAngleDegrees());
    fprintf('DE Path Length: %.2f\n', ...
            double(bestPathMetrics.Length));
    fprintf('DE Path Min Clearance: %.4f\n', ...
            bestPathMetrics.GetMinObstacleDistance());
    fprintf('Angle Improvement: %.2f degrees (%.1f%%)\n', ...
            voronoiPathMetrics.GetMaxVertexAngleDegrees() - bestPathMetrics.GetMaxVertexAngleDegrees(), ...
            100 * (1 - bestPathMetrics.GetMaxVertexAngleDegrees() / voronoiPathMetrics.GetMaxVertexAngleDegrees()));
else
    fprintf('\n⚠ Warning: No feasible solution found!\n');
    fprintf('Best infeasible fitness: %.4f\n', bestFitness);
end

%% Plot convergence
figure('Name', 'Convergence History', 'NumberTitle', 'off');
plot(1:length(fitnessHistory), fitnessHistory, 'LineWidth', 2);
xlabel('Iteration');
ylabel('Best Fitness');
title('Enhanced DE Convergence History');
grid on;
if convergedAt < config.maxIterations
    hold on;
    plot(convergedAt, fitnessHistory(convergedAt), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
    legend('Fitness', sprintf('Converged at %d', convergedAt));
end

%% Helper Functions

function population = lhsInitialization(NP, D)
    % Latin Hypercube Sampling for better initial diversity
    population = zeros(NP, D);
    for d = 1:D
        perm = randperm(NP);
        population(:, d) = (perm' - rand(NP, 1)) / NP;
    end
end

function indices = selectRandomIndices(exclude, NP, count)
    % Select 'count' random indices from 1:NP excluding 'exclude'
    available = [1:exclude-1, exclude+1:NP];
    selected = randperm(length(available), count);
    indices = available(selected);
end

function x = boundaryHandling(x)
    % Reflection boundary handling
    for j = 1:length(x)
        while x(j) < 0 || x(j) > 1
            if x(j) < 0
                x(j) = -x(j);
            elseif x(j) > 1
                x(j) = 2 - x(j);
            end
        end
    end
    x = max(0, min(1, x));  % Final clipping
end

function diversity = calculatePopulationDiversity(population)
    % Calculate average Euclidean distance from centroid
    centroid = mean(population, 1);
    distances = sqrt(sum((population - centroid).^2, 2));
    diversity = mean(distances);
end

function worstIndices = getWorstIndices(fitness, feasible, count)
    % Get indices of worst individuals (prefer infeasible ones)
    if sum(~feasible) >= count
        infeasibleFitness = fitness;
        infeasibleFitness(feasible) = -inf;
        [~, sortedIdx] = sort(infeasibleFitness, 'descend');
        worstIndices = sortedIdx(1:count);
    else
        [~, sortedIdx] = sort(fitness, 'descend');
        worstIndices = sortedIdx(1:count);
    end
end

function [fitness, feasible] = evaluateFitnessAdvanced(gene, voronoiPath, obstacles, config)
    % Advanced fitness evaluation with multi-objective considerations
    feasible = false;
    try
        candidatePath = voronoiPath.CreateFromGene(gene, obstacles, config.pathSafetyDistance);
        candidatePathMetrics = candidatePath.GetMetrics(obstacles);
        clearance = candidatePathMetrics.GetMinObstacleDistance();
        
        if clearance < config.minSafetyDistance
            % Penalize infeasible solutions based on constraint violation
            fitness = 1000 + 1000 * (config.minSafetyDistance - clearance);
            return;
        end
        
        feasible = true;
        maxAngle = candidatePathMetrics.GetMaxVertexAngleDegrees();
        pathLength = double(candidatePathMetrics.Length);
        
        % Normalized objectives
        normalizedAngle = maxAngle / 180.0;
        normalizedLength = pathLength / 100.0;
        normalizedClearance = 1.0 / (clearance + 0.01);
        
        % Multi-objective weighted fitness
        fitness = config.weights.angle * normalizedAngle + ...
                 config.weights.length * normalizedLength + ...
                 config.weights.clearance * normalizedClearance;
    catch
        fitness = inf;
    end
end

function gene = extractVoronoiGene(voronoiPath)
    % Extract normalized gene from Voronoi path
    points = double(voronoiPath.ToArray2D());
    nPoints = size(points, 1);
    
    if nPoints <= 2
        error('Path must have at least 3 points');
    end
    
    interiorPoints = points(2:end-1, :);
    minX = min(points(:, 1));
    maxX = max(points(:, 1));
    minY = min(points(:, 2));
    maxY = max(points(:, 2));
    
    rangeX = max(maxX - minX, 1e-6);
    rangeY = max(maxY - minY, 1e-6);
    
    normalizedX = (interiorPoints(:, 1) - minX) / rangeX;
    normalizedY = (interiorPoints(:, 2) - minY) / rangeY;
    
    nInterior = size(interiorPoints, 1);
    gene = zeros(1, 2 * nInterior);
    gene(1:2:end) = normalizedX';
    gene(2:2:end) = normalizedY';
    gene = max(0, min(1, gene));
end