clear all;
close all;

% Parameters
xt = 0; 
yt = 0; 
v = 1.0; 
d = 1.0; 
z0 = 0; 
k1 = 1;  
k2 = 2.5; 
% kb = 0.75;
r_i = 0.75;
r_o = 1.75;

% ka = 1;
kb = d-r_i;
ka = r_o -d;
%ka = 0.166;
%kb = 1.56;
%theta = pi/2;
% Initial conditions: [x-position; y-position; heading angle; z]
x0 = [1.4; 0.5; 110*pi/180];
%x0 = [1.05; 0.53; 45*pi/180; z0];
%x0 = [1.49; 0.91; 25*pi/180; z0];
%x0 = [1.45; 0.89; 45*pi/180; z0];
% %x0 = [1.0; 0.75; 90.1*pi/180; z0];
% x0 = [-1; 0.6; 240*pi/180; z0];
tspan = [0 50];


opts = odeset('RelTol',1e-5,'AbsTol',1e-7);

% Solve ODE
f = @(t, x) dynamics_unknown(t, x, v, k1, d, k2, ka,kb);
[t, x] = ode45(f, tspan, x0, opts);
control_terms = arrayfun(@(i) calculate_control_term(x(i, :), x(i, 3), v, k1, d, [], ka, kb), 1:size(x, 1));


%%
% Plot Trajectories and Results
figure(1)
%plot(x(:, 1), x(:, 2), 'b', xt, yt, 'ro', 'LineWidth', 1.2);
plot(x(:, 1), x(:, 2), 'b', xt, yt,'r.', 'LineWidth', 1.2, 'MarkerSize', 15);
hold on;
plot(x(1, 1), x(1, 2), 'LineWidth', 2);
%plot(x(1, 1), x(1, 2), 'rx', 'MarkerSize', 8, 'LineWidth', 2);
% viscircles([xt yt], kb, 'Color', [1  0 0], 'LineWidth', 1);
% viscircles([xt yt], ka+d, 'Color', [1  0 0], 'LineWidth', 1);
viscircles([xt yt], r_i, 'Color', [1  0 0], 'LineWidth', 1.5);
viscircles([xt yt], r_o, 'Color', [1  0 0], 'LineWidth', 1.5);
%rectangle('Position', [xt-r_i, yt-r_i, 2*r_i, 2*r_i], 'Curvature', [1, 1], 'EdgeColor', [1 0 0], 'LineWidth', 1.5, 'LineStyle', '-');
%rectangle('Position', [xt-r_o, yt-r_o, 2*r_o, 2*r_o], 'Curvature', [1, 1], 'EdgeColor', [1 0 0], 'LineWidth', 1.5, 'LineStyle', '-');

% axis([-kb-0.3 kb+0.3 -kb-0.3 kb+0.3]);

% axis([-ka-0.3 ka+0.3 -ka-0.3 ka+0.3]);

xlabel('x (m)','Interpreter','latex','fontsize',16);
ylabel('y (m)','Interpreter','latex','fontsize',16);
legend('Robot', 'Target');

xtraj = x(:,1);
ytraj = x(:,2);
theta=x(:,3);
% axes('position', [0.25 0.25 0.5 0.5]);
% box on;
% alpha = 0 < t & t < 300;
% plot(xtraj(alpha), ytraj(alpha), 'b' , 'LineWidth', 2, 'MarkerSize', 5.5);
% hold on;
% plot(x(1, 1), x(1, 2), 'rx', 'MarkerSize', 8, 'LineWidth', 2);
% viscircles([xt yt], ka, 'Color', [1 0 0], 'LineWidth', 1);
% hold off;
% axis([0.66 1.1 0.45 1.05]);
% grid on;
axis square
axis equal
axis([ -2.5 2.5 -2.5 2.5])
hold off;


%%
figure(2);
plot(t, control_terms, 'm', 'LineWidth', 1.2);
hold on;
xlabel('Time (sec)','Interpreter','latex','fontsize',16);
ylabel('Control Term','Interpreter','latex','fontsize',16);
% axis square
% axis equal
axis([ 0 50 -1 2.5])
hold off;
%title('Control Term vs Time');
%%

figure(3);
plot(t, x(:, 3), 'g');
xlabel('Time (sec.)','Interpreter','latex','fontsize',15);
ylabel('Heading Angle ($\theta$)','Interpreter','latex','fontsize',15);
%%
const1 = 1.2 * ones(1, length(t));
figure;
% Plot distance to the target over time
r = sqrt((x(:, 1) - xt).^2 + (x(:, 2) - yt).^2); 


plot(t, r, 'g', 'LineWidth', 1.2);
xlabel('Time (sec)');
hold on;
%plot(t, const1, 'r', 'LineWidth', 1.2);
%yline(ka, 'r.', 'LineWidth', 1.4);
ylabel('r (m)');
axis([0 t(end) 0 ka + 0.75]);
legend('r','k_a');

axes('position', [0.25 0.25 0.5 0.5]);
box on;
alpha = 0 < t & t < 300;
plot(t(alpha), r(alpha), 'g' , 'LineWidth', 2, 'MarkerSize', 5.5);
hold on;
plot(t(alpha), const1(alpha), 'LineWidth', 1.4);
hold off;
axis([0 8 1.12 1.22]);
grid on;
hold off;

figure;
plot(t, x(:, 3), 'g');
xlabel('Time (sec)');
ylabel('Heading Angle (\theta)');
%%
r = sqrt((x(:, 1) - xt).^2 + (x(:, 2) - yt).^2); 
figure;
plot(t, r, 'g', 'LineWidth', 1.2);

hold on;
yline(1.75, 'r-.', 'LineWidth', 2);
yline(0.75, 'r-.', 'LineWidth', 2);

xlabel('Time (sec.)', 'Interpreter', 'latex', 'fontsize', 16);
ylabel('r (m)', 'Interpreter', 'latex', 'fontsize', 16);

axis([0 50 0.5 2]);
hold off;
%%

%k = sqrt((x(:, 1)-d*sin(theta)- xt).^2 + (x(:, 2)+d*cos(theta)- yt).^2); 
% pk = r-d;
% figure(6);
% plot(t, pk, 'g', 'LineWidth', 1.2);
% xlabel('Time (sec)','Interpreter','latex','fontsize',16);
% ylabel('Error($\bar{p}_k$)','Interpreter','latex','fontsize',16);
% hold on;
% yline(ka, 'r-.', 'LineWidth', 2);
% ylabel('r (m)');
% axis([0 t(end) 0 ka + 0.5]);

% for i=1:length(r)
%     if r(i)>1 && 
%         q=1;
%         blf(i)= q*((1/d)*(v+(((r-d)*(v/r))/(d*k1*(ka^2-(r-d)^2)));
%     else
%         q=0;
%         blf(i)= (1-q)*((1/d)*(v+(((r-d)*(v/r))/(d*k1*(kb^2-(r-d)^2)))+v*c));
%     end
% end
% figure
% plot(t,blf)
% 
%     px = cos(theta)*(x1-d*sin(theta)- 0) + sin(theta)*(y1+d*cos(theta)- 0);
%     pk = r-d;
%    sigma = @(s) s ;
%     c = sigma(v*px);
% for i = 1:length(r)
%     if r(i) > 1
%         q = 1;
%         blf(i) = q * ((1/d) * (v + (((r(i) - d) * (v / r(i))) / (d * k1 * (ka^2 - (r(i) - d)^2)))));
%     else
%         q = 0;
%         blf(i) = (1 - q) * ((1/d) * (v + (((r(i) - d) * (v / r(i))) / (d * k1 * (kb^2 - (r(i) - d)^2)))) + v * c);
%     end
% end
% 
% figure;
% plot(t, blf);
%%
desired_distance = d;
actual_distance = sqrt((x(:, 1) - xt).^2 + (x(:, 2) - yt).^2);
error = actual_distance - desired_distance;
figure;
plot(t, error, 'b', 'LineWidth', 1.2);
xlabel('Time (sec)', 'Interpreter', 'latex', 'fontsize', 16);
ylabel('Position Error (m)', 'Interpreter', 'latex', 'fontsize', 16);
title('Error over Time', 'Interpreter', 'latex', 'fontsize', 16);
axis([ 0 50 -0.2 0.6]) 
%%
% Function definitions must be at the end of the script
function dxdt = dynamics_unknown(~, x, v, k1, d, k2, ka, kb)
    % States
    x_p = x(1); 
    y_p = x(2); 
    theta = x(3); 
    z = x(4); 

    % Calculate the control term
    control_term = calculate_control_term(x, theta, v, k1, d,k2, ka,kb);
    
    z_dot = -k2 * z + 0.5 * (x_p^2 + y_p^2);

    dxdt = [
        v * cos(theta); % dx/dt
        v * sin(theta); % dy/dt
        control_term;   % dtheta/dt
        z_dot           % dz/dt
    ];
end

function control_term = calculate_control_term(x, theta, v, k1,d, k2,ka, kb)
    % States
    x1 = x(1); 
    y1 = x(2); 
    
    %xp=x1-d*sin(theta);
    %yp=y1+d*cos(theta);

    %xt=0;
    %yt=0;

    %e_x=xp-0;
    %e_y=yp-0;

    
    z = x(4); 
    %ke = sqrt((x1-d*sin(theta)- 0).^2 + (y1+d*cos(theta)- 0).^2); 
    % Error terms
    %e_x = x_p - 0; % Target x-coordinate is 0
    %e_y = y_p - 0; % Target y-coordinate is 0
    %radius = sqrt(e_x^2 + e_y^2 + d^2 + 2*d*(e_x*sin(theta) - e_y*cos(theta)));
    r=sqrt(x1.^2+y1.^2);%&& radius > 1
   % s_fun = sqrt(e_x^2 + e_y^2);
    pk = r-d;
    if pk > 0
         q = 1;
    else
         q = 0;
    end
    %px = cos(theta)*(x1-d*sin(theta)- 0) + sin(theta)*(y1+d*cos(theta)- 0);
    % Sigma function
    sigma = @(s) s ;%/ sqrt(1 + s^2);
    c = sigma(-k2 * z + 0.5 * (x1^2 + y1^2));
    control_term = q*((1/d)*(v+(((r-d)*(v/r))/(d*k1*(ka^2-(r-d)^2)))+v*c))+(1-q)*((1/d)*(v+(((r-d)*(v/r))/(d*k1*(kb^2-(r-d)^2)))+v*c));
    %control_term = q*((1/d)*(v+(((r-d)*(v/r))/(d*k1*(ka^2-(r-d)^2)))+px))+(1-q)*((1/d)*(v+(((r-d)*(v/r))/(d*k1*(kb^2-(r-d)^2)))));
    %control_term = q*((k1 * v * c / (ka^2 - ke^2)) + (v / d)) + (1 - q) * ((k1 * v * c / (kb^2 - ke^2)) + (v / d));
    %control_term = q*((k1 * v * c / (ka^2 - e_x^2 - e_y^2)) + (v / d)) + (1 - q) * ((k1 * v * c / (kb^2 - e_x^2 - e_y^2)) + (v / d));
end
>> euler_circumnavigation
Index exceeds the number of array elements. Index must not exceed 3.

Error in euler_circumnavigation>dynamics_unknown (line 202)
    z = x(4);

Error in euler_circumnavigation>@(t,x)dynamics_unknown(t,x,v,k1,d,k2,ka,kb) (line 35)
f = @(t, x) dynamics_unknown(t, x, v, k1, d, k2, ka,kb);

Error in odearguments (line 92)
f0 = ode(t0,y0,args{:});   % ODE15I sets args{1} to yp0.

Error in ode45 (line 104)
    odearguments(odeIsFuncHandle,odeTreatAsMFile, solver_name, ode, tspan, y0, options, varargin);

Error in euler_circumnavigation (line 36)
[t, x] = ode45(f, tspan, x0, opts);
 
>> please resolve above the code rewrite the correct code