% 初始化参数
numWhales = 30; % 子鲸数量
maxIterations = 100; % 最大迭代次数
mapSize = [100, 100, 100]; % 地图尺寸

startPoint = [5, 5, 5]; % 起点坐标
endPoint = [95, 95, 95]; % 终点坐标

% 创建地图和障碍物
figure;
axis([0 mapSize(1) 0 mapSize(2) 0 mapSize(3)]);
hold on;
xlabel('x');
ylabel('y');
zlabel('z');
view(3);
title('Path Planning');
grid on;

obstacles = createObstacles(); % 创建障碍物

% 初始化鲸鱼位置
whalePositions = initializeWhalePositions(numWhales, mapSize);

% 主循环
fitnessHistory = zeros(maxIterations, 1);
bestPath = [];
for iter = 1:maxIterations
    % 计算适应度
    fitness = calculateFitness(whalePositions, startPoint, endPoint, obstacles, mapSize);
    
    % 保存最佳适应度值
    bestFitness = min(fitness);
    fitnessHistory(iter) = bestFitness;
    
    % 更新鲸鱼位置
    whalePositions = updateWhalePositions(whalePositions, bestFitness, mapSize);
    
    % 可视化最优路线
    if iter == maxIterations
        bestIndex = find(fitness == bestFitness, 1);
        bestPath = findPath(startPoint, endPoint, obstacles, whalePositions(i, :), mapSize);
        
    end
     % 添加鲸鱼位置到路径中
    whalePath = repmat(whalePositions, size(bestPath, 1), 1);
    path = [bestPath, whalePath];
    
    % 可视化路径
    plotObstacles(obstacles); % 绘制障碍物
    plotBestPath(path);
    
end
%绘制适应度变化图
figure;
plot(1:iter, fitnessHistory(1:iter));
xlabel('Iteration');
ylabel('Fitness');
title('Fitness Evolution');

% 创建障碍物的函数
function obstacles = createObstacles()
    % 创建8个不相互接触的障碍物
    obstacle1 = createBoxObstacle([20, 20, 20], [30, 40, 50]);
    obstacle2 = createBoxObstacle([60, 60, 60], [70, 80, 90]);
    obstacle3 = createSphereObstacle([35, 80, 25], 10);
    obstacle4 = createSphereObstacle([25, 50, 70], 7);
    obstacle5 = createBoxObstacle([5, 85, 55], [15, 95, 65]);
    obstacle6 = createBoxObstacle([75, 45, 15], [85, 55, 25]);
    obstacle7 = createSphereObstacle([90, 70, 30], 8);
    obstacle8 = createSphereObstacle([40, 20, 85], 5);
    
    obstacles = [obstacle1, obstacle2, obstacle3, obstacle4, obstacle5, obstacle6, obstacle7, obstacle8];
end
% 绘制障碍物的函数
function plotObstacles(obstacles)
    for i = 1:length(obstacles)
        vertices = obstacles(i).vertices;
        faces = obstacles(i).faces;
        patch('Vertices', vertices, 'Faces', faces, 'FaceColor', 'red');
    end
    view(3); % 设置视角
end


% 创建长方体形状的障碍物函数
function obstacle = createBoxObstacle(lowerCorner, upperCorner)
    vertices = [
        lowerCorner;
        lowerCorner(1), upperCorner(2), lowerCorner(3);
        upperCorner(1), upperCorner(2), lowerCorner(3);
        upperCorner(1), lowerCorner(2), lowerCorner(3);
        lowerCorner(1), upperCorner(2), upperCorner(3);
        upperCorner(1), upperCorner(2), upperCorner(3);
        upperCorner;
        upperCorner(1), lowerCorner(2), upperCorner(3);
    ];
    
    faces = [
        1, 2, 3, 4;
        2, 5, 6, 3;
        5, 7, 8, 6;
        7, 1, 4, 8;
        6, 8, 4, 3;
        7, 5, 2, 1;
    ];
    
    obstacle.vertices = vertices;
    obstacle.faces = faces;
end
% % 创建长方体形状的障碍物函数
% function obstacle = createBoxObstacle(lowerCorner, upperCorner)
%     vertices = [
%         lowerCorner;
%         lowerCorner(1), upperCorner(2), lowerCorner(3);
%         upperCorner;
%         upperCorner(1), lowerCorner(2), upperCorner(3);
%         lowerCorner(1), upperCorner(2), upperCorner(3);
%         upperCorner(1), upperCorner(2), lowerCorner(3);
%     ];
%     
%     faces = [
%         1, 2, 3, 4;
%         2, 5, 3, 3;
%         5, 6, 3, 1;
%         6, 1, 3, 1;
%         6, 5, 4, 4;
%         5, 2, 4, 6;
%     ];
%     
%     obstacle.vertices = vertices;
%     obstacle.faces = faces;
% end
% 创建球体形状的障碍物函数
function obstacle = createSphereObstacle(center, radius)
    [x, y, z] = sphere;
    vertices = radius * [x(:), y(:), z(:)];
    vertices = unique(vertices + center, 'rows');
    
    obstacle.vertices = vertices;
    obstacle.faces = delaunay(vertices(:, 1), vertices(:, 2), vertices(:, 3));
end

% 初始化鲸鱼位置的函数
function whalePositions = initializeWhalePositions(numWhales, mapSize)
    whalePositions = rand(numWhales, 3) .* mapSize;
end

% 计算适应度的函数
function fitness = calculateFitness(whalePositions, startPoint, endPoint, obstacles, mapSize)
    numWhales = size(whalePositions, 1);
    fitness = zeros(numWhales, 1);
    
    for i = 1:numWhales
        path = findPath(startPoint, endPoint, obstacles, whalePositions(i, :), mapSize);
        fitness(i) = calculatePathLength(path);
    end
end

% 更新鲸鱼位置的函数
function newWhalePositions = updateWhalePositions(whalePositions, bestFitness, mapSize)
    numWhales = size(whalePositions, 1);
    newWhalePositions = zeros(size(whalePositions));
    
    for i = 1:numWhales
        delta = randn(1, 3) .* (1 - i / numWhales);
        delta = delta * bestFitness / norm(delta);
        newPos = whalePositions(i, :) + delta;
        
        % 限制新位置在地图范围内
        newPos(newPos < 0) = 0;
        newPos(newPos > mapSize) = mapSize(newPos > mapSize);
        
        newWhalePositions(i, :) = newPos;
    end
end

% 找到最佳路线的函数
% function path = findPath(startPoint, endPoint, obstacles, whalePosition)
%     % TODO: 使用路径规划算法找到从起点到终点的最佳路线
%     % 这里仅作示意，假设最佳路线就是直线
%     path = [startPoint; endPoint];
% end
function path = findPath(startPoint, endPoint, obstacles, whalePosition, mapSize)
    % 创建地图
    map = robotics.BinaryOccupancyGrid(mapSize(1), mapSize(2), mapSize(3));
    
    % 将障碍物添加到地图中
    for i = 1:length(obstacles)
        vertices = obstacles(i).vertices;
        faces = obstacles(i).faces;
        addCollision(map, vertices, faces);
    end
    
    % 将起点和终点添加到地图中
    setOccupancy(map, startPoint, 1);
    setOccupancy(map, endPoint, 1);
    
    % 创建起点和终点的网格坐标
    startGrid = world2grid(map, startPoint);
    endGrid = world2grid(map, endPoint);
    
    % 使用A算法进行路径规划
    gridSize = map.Resolution;
    heuristicFcn = @(state) norm(state - endGrid);
    costFcn = @(state1, state2) norm(state2 - state1);
    [optimalPath, ~] = astar(map, startGrid, endGrid, 'HeuristicFcn', heuristicFcn, 'CostFcn', costFcn);
    
    % 将网格坐标转换回世界坐标
    path = optimalPathgridSize + gridSize/2;
    
    % 添加鲸鱼位置到路径中
    path = [path; whalePosition];
end

% 绘制最佳路线的函数
function plotBestPath(bestPath)
    plot3(bestPath(:, 1), bestPath(:, 2), bestPath(:, 3), 'b', 'LineWidth', 2);
end

% 计算路径长度的函数
function pathLength = calculatePathLength(path)
    pathLength = sum(vecnorm(diff(path), 2, 2));
end