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.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 200];


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);

% Plot Trajectories and Results
figure;
plot(x(:, 1), x(:, 2), 'b', xt, yt, 'ro', 'LineWidth', 1.2);
hold on;
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);
viscircles([xt yt], r_o, 'Color', [1  0 0], 'LineWidth', 1);
% axis([-kb-0.3 kb+0.3 -kb-0.3 kb+0.3]);
axis equal;
axis([-1.5 1.5 -2 2]);
% 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','Initial position');

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 equal;
hold off;

figure;
plot(t, control_terms, 'm', 'LineWidth', 1.2);
xlabel('Time (sec)');
ylabel('Control Term');
title('Control Term vs Time');


figure;
plot(t, x(:, 3), 'g');
xlabel('Time (sec)');
ylabel('Heading Angle (\theta)');

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);
xlabel('Time (sec)');
hold on;
yline(ka, 'r-.', 'LineWidth', 2);
ylabel('r (m)');
axis([0 t(end) 0 ka + 0.5]);


%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);



% 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, ~,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 ;
    c = sigma(v*px);
    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*((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
plot between controller term vs time