clc
clear all
close all

% Initialization
l = 0.1;
y = 0;
x = -0.5;
theta = -30 *pi/180;

% Heading - P controller
% ------------------------
Kheading_p = 0.2;
Kheading_d = 0.0;
Kheading_i = 0.1;
headingError_prev = 0;
integralHeading = 0;
dphi = 0;

% Forward motion - PD controller
% ------------------------
v = 0.0;
dv = 0;
Kforward_p = 1;
Kforward_d = 0.5;
Kforward_i = 0.1;
distanceError_prev = 0;
integralForward = 0;

% Square trajectory
R = 1;
x0 = 1;
y0 = 0;
zeta = pi;
dzeta = -0.2;
aheadzeta = -pi/20;

% Simulation
dt = 0.1;
t_end = 30;
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);
headingError_array = zeros(nSteps,1);
distanceError_array = zeros(nSteps,1);
wLcoords = zeros(nSteps,2);
wRcoords = zeros(nSteps,2);
for ii = 1:nSteps
    % We need to correct the angle theta so it stays between -pi and +pi to
    % avoid errors.
    if theta < - pi
        theta = theta + 2*pi;
    end
    if theta > + pi
        theta = theta - 2*pi;
    end
    % Square trajectory
    % ---------------------------------------------------------------------
    Q = [x0; y0] + R*[cos(zeta); sin(zeta)] * min(abs(1/cos(zeta)),abs(1/sin(zeta)));
    Qahead = [x0; y0] + R*[cos(zeta + aheadzeta); sin(zeta + aheadzeta)] * min(abs(1/cos(zeta + aheadzeta)),abs(1/sin(zeta + aheadzeta)));
    
    % CONTROL
    % =====================================================================
    
    % Forward drive control
    % ---------------------------------------------------------------------
    % Calculate the error as the normal distance from the wheelbase to the
    % point Q on the trajectory.
    % Calculate the distance in local frame
    OQ_loc = [cos(theta) sin(theta); -sin(theta) cos(theta)]*(Q-[x;y]);
    distanceError = OQ_loc(1); % Get the x value in the local frame
    % Calculate the derivative, and integral errors
    derivativeForward = (distanceError - distanceError_prev)/dt;
    integralForward = integralForward + distanceError * dt;
    % Calculate the velocity control of the PID
    velocityError = Kforward_p * distanceError + Kforward_d * derivativeForward + Kforward_i * integralForward;
    vcontrolled = velocityError;
    distanceError_prev = distanceError;
    
    % Heading control
    % ---------------------------------------------------------------------
    % The error is the angular difference between the orientation of the
    % robot and the desired heading direction (both in world frame)
    desiredHeading = atan2(Qahead(2)-y, Qahead(1)-x);
    headingError = desiredHeading - theta;
    % We need to correct the angle headingError so it stays between -pi and
    % +pi to avoid errors.
    if headingError > pi
        headingError = headingError - 2*pi;
    end
    if headingError < -pi
        headingError = headingError + 2*pi;
    end
    % Calculate the derivative, and integral errors
    derivativeHeading = (headingError - headingError_prev)/dt;
    integralHeading = integralHeading + headingError * dt;
    % Calculate the heading control of the PID
    delta = Kheading_p * headingError + Kheading_d * derivativeHeading + Kheading_i * integralHeading;
    
    % Control the velocity vector
    % ---------------------------------------------------------------------
    % Now we can apply the base velocity v and the delta velocity to the
    % wheels. The composition of the two velocities (v and delta) produces
    % the motion of the robot, in the correct direction.
    v_R = v + delta;
    v_L = v - delta;
    headingError_prev = headingError;
    
    
    % KINEMATICS MODEL
    % =====================================================================
    dq = [1/2*cos(theta) ; 1/2*sin(theta) ;  1/(2*l)] * v_R + ...
         [1/2*cos(theta) ; 1/2*sin(theta) ; -1/(2*l)] * 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;
    zeta = zeta + dzeta*dt;
    v = vcontrolled;
    
    % Calculate position of wheels
    % ---------------------------------------------------------------------
    xwL = x + l*cos(theta+pi/2);
    ywL = y + l*sin(theta+pi/2);
    xwR = x + l*cos(theta-pi/2);
    ywR = y + l*sin(theta-pi/2);
    
    % 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];
    headingError_array(ii) = headingError;
    distanceError_array(ii) = distanceError;
    
    % Plot
    % ---------------------------------------------------------------------
    if mod(ii,1) == 0
        cla
        x2 = x + l*cos(theta); % "fictional" front of the robot
        y2 = y + l*sin(theta);
        plot([x x2],[y y2],'lineWidth',2,'color',[0 0.4470 0.7410]);       % Plots the robot body
        hold on;
        plot(Q(1),Q(2),'ok');           % Plots the target point
        plot(Qahead(1),Qahead(2),'ok');           % Plots the target point
        plot([x Qahead(1)],[y Qahead(2)],'-k');   % Plots line to the target point
        plot([xwR xwL],[ywR ywL],'lineWidth',2,'color',[0 0.4470 0.7410]); % Plots the wheel axis
        plot(xs(1:ii),ys(1:ii),'color',[0.8500 0.3250 0.0980]); % Plots the robot trajectory
        plot(wLcoords(1:ii,1),wLcoords(1:ii,2),'color',[0.9290 0.6940 0.1250]); % Plot the left wheel trajectory
        plot(wRcoords(1:ii,1),wRcoords(1:ii,2),'color',[0.9290 0.6940 0.1250]); % Plot the right wheel trajectory
        plot([x0-R x0+R x0+R x0-R x0-R],[y0+R y0+R y0-R y0-R y0+R],':b'); % Plot desired square path
        xlim([-1.0 2.5])
        ylim([-1.5 1.5])
        t = t_array(ii);
        title(sprintf('Time: %.f s',t))
        daspect([1 1 1])
        drawnow
    end
end

figure
plot(t_array, headingError_array);
hold on
plot(t_array, distanceError_array);
xlabel('Time [s]')
grid on
title('Error');