%% 最小二乘问题

A = randn(100, 100);
b = randn(100, 1);

xs = random_search(@(x)least_squares(x,A,b), b, 1000, @gss);

% 绘制误差图
ys = arrayfun(@(i) least_squares(xs(:, i),A,b), 1:size(xs, 2));
plot(ys);
xlabel('Iteration');
ylabel('Error');
title('Error Plot');

%% 非凸函数-随机搜索
points = linspace(-5, 5, 100);
[X, Y] = meshgrid(points, points); % 生成网格数据
Z = zeros(size(X));
for i=1:numel(X)
    vector_input=[X(i);Y(i)];
    Z(i)=himmelblau(vector_input);
end
fig = figure('Position', [100, 100, 800, 800]); 
ax = axes('Parent', fig, 'Projection', 'orthographic'); 

surf(ax, X, Y, Z, 'FaceColor', 'interp', 'EdgeColor', 'none');
xlabel('X'); 
ylabel('Y'); 
zlabel('Z'); 
title('Himmelblau''s Function'); % 图形标题

xs = random_search(@himmelblau,[1;1], 50);
ys = arrayfun(@(i) himmelblau(xs(:, i)), 1:size(xs, 2));
figure;
plot(ys);
xlabel('Iteration');
ylabel('Error');
title('Error Plot');

%% 非凸函数-Nelder-Mead法
options = optimset('Display','iter','PlotFcns',@myplot);
x0 = [1;1];
fun = @himmelblau;
[x,fval,exitflag,output] = fminsearch(fun,x0,options)


%% Flappy Bird基本仿真

% 初始化Flappy Bird的初始状态和动作
init_action = 0;
init_state = [0;10];
num_steps = 10; % 定义展开的步数

[states, actions] = unroll(@flappy_bird, init_state,init_action, @example_policy, num_steps);

heights = states(2,:);

fig = figure('Position', [100, 100, 750, 500]); 
ax = axes('Parent', fig); 

plot(ax, 1:length(heights), heights, 'LineWidth', 2);

ylabel(ax, 'height');
xlabel(ax, 'time');

num_steps = 20; % 定义展开的步数

[states, actions] = unroll(@flappy_bird, init_state,init_action, @example_policy, num_steps);

heights = states(2,:);

fig = figure('Position', [100, 100, 750, 500]);
ax = axes('Parent', fig); 

plot(ax, 1:length(heights), heights, 'LineWidth', 2);

ylabel(ax, 'height');
xlabel(ax, 'time');

%% Flappy Bird 强化学习


% 执行随机搜索

for i=1:9
    
    xs = random_search(@objective, zeros(4, 1), 10, @gss);
    
    % 创建策略函数
    policy = @(states, actions) flappy_policy(xs(:,end), states, actions);
    
    init_action = 0;
    init_state = [0;10];
    [states, actions] = unroll(@flappy_bird, init_state,init_action, policy, 20);
    
    heights = states(2,:);

    subplot(3,3,i);
    plot(1:length(heights), heights, 'LineWidth', 2);
    
    ylabel('height');
    xlabel('time');    
end

%%
function x_min = gss(f, a, b, tol)
    % 黄金分割搜索
    % 求函数f在[a,b]之间的最小值
    % f: 函数句柄，待求函数
    % a: 实数，区间下界
    % b: 实数，区间上界
    % tol: 收敛容差
    
    if nargin < 4
        tol = 1e-5; 
    end
    
    gr = (sqrt(5) + 1) / 2;
    c = b - (b - a) / gr;
    d = a + (b - a) / gr;
    
    while abs(c - d) > tol
        if f(c) < f(d)
            b = d;
        else
            a = c;
        end
        c = b - (b - a) / gr;
        d = a + (b - a) / gr;
    end
    x_min = (b + a) / 2;
end

function iterates = random_search(oracle, init, num_steps, line_search)
    % 实现随机搜索。
    
    % oracle: 函数句柄，待求函数。
    % init: 列向量，oracle 定义域中的点，迭代起始点。
    % num_steps: 整数，迭代次数。
    % line_search: 线搜索方法（默认为黄金分割搜索）。
    
    if nargin < 4
        line_search = @gss;
    end
    
    iterates = [init];
    for i = 1:num_steps
        d = randn(size(init)) .* 100;
        d = d / norm(d);
        x = iterates(:,end);
        eta = line_search(@(step) oracle(x + step * d), -1, 1);
        iterates = [iterates, x + eta * d];
        size(iterates)
    end
end

function error = least_squares(x, A, b)
    % 计算最小二乘误差。
    
    % x: 列向量，自变量
    % A: 矩阵
    % b: 列向量
    
    error = 0.5 * norm(A * x - b)^2;
end

function ans=himmelblau(X)
    % 非凸函数
    % x: 列向量，自变量
    ans = (X(1).^2 + X(2) - 11).^2 + (X(1) + X(2).^2 - 7).^2;
end


function stop = myplot(x,optimValues,state)
    stop = false;
    persistent points steps;
    switch state
        case 'init'
              % Setup for plots or dialog boxes
              hold on
        case 'iter'
              % Make updates to plots or dialog boxes as needed
              plot(optimValues.iteration,optimValues.fval,'bx');
              points=[points,optimValues.fval];
              steps = [steps,optimValues.iteration];
        case 'done'
              % Cleanup of plots, dialog boxes, or final plot
              plot(steps,points);
              hold off
    end
end

function [new_state] = flappy_bird(state, action)
    % Flappy bird 动态系统。
    % 输入: 
        % state: 2维列向量，下降速度和高度
        % action: 实数，向上加速度
    % 返回: 
        % new_state: 列向量，新的下降速度和高度
        
    v = state(1); % 下降速度
    h = state(2); % 高度
    g = 1; % 重力加速度
    k = 0.01; % 阻力系数
    m = 1; % 质量
    new_state = [v + g - (k/m)*v - action; h - v]; % 计算新的速度和高度
end

function [states, actions] = unroll(system, init_state,init_action, policy, num_steps)
    % 展开动态系统。
    % 输入: 
        % system: 函数句柄，动力系统
        % init_state: 2维列向量，动力系统状态（速度和高度）
        % init_action: 实数，动力系统状态（加速度）
        % policy: 函数句柄，运动策略
        % num_steps: 整数，模拟的长度
    % 返回: 
        % states: 2行num_steps列矩阵，整个过程中的速度和高度
        % actions: num_steps维行向量，整个过程中的加速度
        
    states = init_state; % 初始化状态列表
    actions = init_action; % 初始化动作列表
    for i = 1:num_steps
        action = policy(states, actions); % 根据策略函数确定动作
        actions = [actions, action]; % 将动作添加到动作列表
        states = [states, system(states(:,end), action)]; % 更新状态
    end
end

function action = example_policy(states, actions)
    % 每5步加速向上。
        % 展开动态系统。
    % 输入: 
        % states: 2行k列矩阵，迄今为止的速度和高度
        % actions: k维行向量，迄今为止的加速度
    % 返回: 
        % action: 实数，动作完成后的加速度
        
    v = states(1,end); % 最后一个状态的速度
    h = states(2,end); % 最后一个状态的高度
    if mod(length(states), 5) == 0
        action = 7; % 每5步执行一次向上加速
    else
        action = 0; % 否则不加速
    end
end

function reward = flappy_reward(states, actions)
    % Flappy bird游戏的奖励函数。
     % 输入: 
        % states: 2行k列矩阵，迄今为止的速度和高度
        % actions: k维行向量，迄今为止的加速度
    % 返回: 
        % reward: 实数，奖励值
        
    reward = 0.0;
    for i = 1:size(states, 2)
        state = states(:, i);
        action = actions(i);
        height = state(2);
        if height < 0
            reward = reward + (10 * height-10);
        end
        reward = reward - abs(action);
    end
end

function action = flappy_policy(theta, states, actions)
    % 策略函数
    % 输入: 
        % theta: 4维列向量，加权系数
        % states: 2行k列矩阵，迄今为止的速度和高度
        % actions: k维行向量，迄今为止的加速度（实际未使用）
    % 输出: 
        % action: 实数，动作完成后的加速度

    if size(states, 2) == 1
        states = [states, states];
    end
    state_vector = states(:,end-1:end); % 取最后两个状态
    action = theta' * reshape(state_vector, [], 1); %用theta进行加权
end

function reward = flappy_simulator(theta, num_steps)
    % 计算给定步数的策略奖励。
    % 输入: 
        % theta: 4维列向量，加权系数
        % num_steps: 仿真长度
    % 输出:
        % reward: 本轮仿真获得的奖励值
    policy = @(sts, ats) flappy_policy(theta, sts, ats);
    init_action = 0;
    init_state = [0;10];
    [states, actions] = unroll(@flappy_bird, init_state,init_action, policy, num_steps);
    reward = flappy_reward(states, actions);
end

function reward=objective(theta)
    reward=(-1.0)*flappy_simulator(theta,20);
end