clc
clear all
close all

% Initialization
phi = 0 *pi/180;
v = 0.1;
d = 0.1;
wheel_radius = 0.05; % Radius of the wheels (only used for display purposes)
y = 0;

% % PD controller
% % ------------------
% theta = 45 *pi/180;
% x = 0;
% K_p = 0.5*1.0;
% K_d = 1*1.0;
% K_i = 0*0.1;

% PID controller
% ------------------
theta = 90 *pi/180;
x = -0.5;
K_p = 1.0;
K_d = 4.0;
K_i = 0.1;

dphi_max = 0.1;
radialError_prev = 0;
integral = 0;
dphi = 0;
% Circle
R = 1;
x0 = 1;
y0 = 0;

% Simulation
dt = 0.1;
t_end = 100;
t = 0;
t_array = 0:dt:t_end;
nSteps = numel(t_array);

% Initialize results array
xs = zeros(nSteps,1);
ys = zeros(nSteps,1);
vRs = zeros(nSteps,1);
vLs = zeros(nSteps,1);
radialError_array = zeros(nSteps,1);
wLcoords = zeros(nSteps,2);
wRcoords = zeros(nSteps,2);
for ii = 1:nSteps
    % Control
    if theta < - pi
        theta = theta + 2*pi;
    end
    desiredRadius = R;
    radialError = desiredRadius - norm([x;y] - [x0;y0]);
    derivative = (radialError - radialError_prev)/dt;
    integral = integral + radialError * dt;
    delta = K_p * radialError + K_d * derivative + K_i * integral;
    v_R = v + delta;
    v_L = v - delta;
    radialError_prev = radialError;
    
    % Kinematics model
    dq = [1/2*cos(theta) ; 1/2*sin(theta) ;  1/(2*d)] * v_R + ...
         [1/2*cos(theta) ; 1/2*sin(theta) ; -1/(2*d)] * v_L;
    dx = dq(1);
    dy = dq(2);
    dtheta = dq(3);
    
    % Time integration
    x = x + dx*dt;
    y = y + dy*dt;
    theta = theta + dtheta*dt;
    
    % Calculate position of wheels
    xwL = x + d*cos(theta+pi/2);
    ywL = y + d*sin(theta+pi/2);
    xwR = x + d*cos(theta-pi/2);
    ywR = y + d*sin(theta-pi/2);
    % Coordinates of wheels (display purposes only)
    x1l = x - d * cos(theta+pi/2) + wheel_radius * cos(theta); % Left wheel tip x-coordinate (1)
    y1l = y - d * sin(theta+pi/2) + wheel_radius * sin(theta); % Left wheel tip y-coordinate (1)
    x2l = x - d * cos(theta+pi/2) - wheel_radius * cos(theta);
    y2l = y - d * sin(theta+pi/2) - wheel_radius * sin(theta);
    x1r = x + d * cos(theta+pi/2) + wheel_radius * cos(theta);
    y1r = y + d * sin(theta+pi/2) + wheel_radius * sin(theta);
    x2r = x + d * cos(theta+pi/2) - wheel_radius * cos(theta);
    y2r = y + d * sin(theta+pi/2) - wheel_radius * sin(theta);
    
    % Save in array
    xs(ii) = x;
    ys(ii) = y;
    vRs(ii) = v_R;
    vLs(ii) = v_L;
    wLcoords(ii,:) = [xwL ywL];
    wRcoords(ii,:) = [xwR ywR];
    radialError_array(ii) = radialError;
    
    % Plot
    if mod(ii,10) == 0
        cla
        x2 = x + d*cos(theta); % "fictional" front of the robot
        y2 = y + d*sin(theta);
        plot(wLcoords(1:ii,1),wLcoords(1:ii,2),'color',[0.9290 0.6940 0.1250]); % Plot the left wheel trajectory
        hold on;
        plot(wRcoords(1:ii,1),wRcoords(1:ii,2),'color',[0.9290 0.6940 0.1250]); % Plot the right wheel trajectory
        plot(xs(1:ii),ys(1:ii),'color',[0.8500 0.3250 0.0980]); % Plots the robot trajectory
        plot([x x2],[y y2],'lineWidth',2,'color',[0 0.4470 0.7410]);       % Plots the robot body
        plot([xwR xwL],[ywR ywL],'lineWidth',2,'color',[0 0.4470 0.7410]); % Plots the wheel axis
        plot([x1l x2l],[y1l y2l], 'lineWidth', 2,'color','k'); % Rear wheel
        plot([x1r x2r],[y1r y2r], 'lineWidth', 2,'color','r'); % Front wheel
        circle(x0,y0,R,':b'); % Plot desired circular path
        xlim([-1.0 2.5])
        ylim([-1.5 1.5])
        daspect([1 1 1])
        drawnow
    end
end

figure
subplot(2,1,1)
plot(t_array, vRs);
hold on
plot(t_array, vLs);
xlabel('Time [s]')
grid on
legend('v_R','v_L');
title('Wheel speed');

subplot(2,1,2)
plot(t_array, radialError_array);
xlabel('Time [s]')
grid on
title('Error');