close all;
clear all;
clc;

width = 10;
height = width;

% Set up figure window, plot the grid
figure
hold on;
xlim([-width/2 width/2])
ylim([-height/2 height/2])
xticks([-width/2:1:width/2])
yticks([-height/2:1:height/2])
axis square
grid on;

turningPoints = [];
turningPoints(:,:,1) = [0.5, 0.1];
direction_rad = pi;

m = 0;
b = 0.1;

V = 0;
L = 0;

plot(turningPoints(1,1,1), turningPoints(1,2,1), 'or');
% Plot the "mirrors" (i.e. plot circles)
for x = [-width/2:1:width/2]
   for y = [-height/2:1:height/2]
       c = viscircles([x y],1/3,'Color','K');
       fill(c.Children(1).XData(1:end-1), c.Children(1).YData(1:end-1), [0.7294 0.7294 0.7294]);
   end
end

intersection = cl_intersection(V, L, m, b);

line([turningPoints(1,1,1), intersection(1,1,1)],[turningPoints(1,2,1), intersection(1,2,1)], 'Color', [61/255 165/255 244/255], 'LineStyle', '--', 'LineWidth', 2);

distanceTravelled = 0;

angle=pi;
globalAngle = angle;

while (distanceTravelled < 10)
    sinAngle = sin(globalAngle)
    cosAngle = cos(globalAngle)
    if (sinAngle < 2e-16 && sinAngle > -2e-16)
        sinAngle = 0;
    end
    if (cosAngle < 2e-16 && cosAngle > -2e-16)
        cosAngle = 0;
    end



   % from the previous turning point, determine which mirror is the next one to be hit by our laser
   if (cosAngle < 0) %if we are moving left
       if (sinAngle < 0)     % if we are moving left AND down

       elseif (sinAngle > 0) % if we are moving left AND up
           % start looking for first intersection

           %check the mirror directly above
           intersection = cl_intersection(V + 1, L, m, b);

           % if there was no intersection, check the mirror to the left
           if (~isreal(intersection(:,:,1)) && ~isreal(intersection(:,:,2)))
                intersection = cl_intersection(V, L - 1, m, b);
           end

           %check the remaining possible mirrors
           if (~isreal(intersection(:,:,1)) && ~isreal(intersection(:,:,2)))
               for i = 1:1:20
                    intersection = cl_intersection(V + 1, L - i, m, b);
                    if (~isreal(intersection(:,:,1)))
                        if (~isreal(intersection(:,:,2)))
                            intersection = cl_intersection(V + 1 + i, L - 1, m, b);
                            continue;
                        end
                    end

                   d1 = sqrt((turningPoints(1,1,end) - intersection(1,1,end-1))^2 + (turningPoints(1,2,end) - intersection(1,2,end-1))^2);
                   d2 = sqrt((turningPoints(1,1,end) - intersection(1,1,end))^2 + (turningPoints(1,2,end) - intersection(1,2,end))^2);

                   if (d1 < d2) % if d1 is closer than d2, d1 is our intersection
                       turningPoints = [turningPoints; intersection(:,:,end-1)];
                   else % d2 is closer than d1
                       turningPoints = [turningPoints; intersection(:,:,end)];
                   end
                   break;
               end
           end



       else                  % we are not moving up or down (purely leftwards)
           cx = floor(turningPoints(1,1,end));
           cy = floor(turningPoints(1,2,end));
           intersection = cl_intersection(cx, cy, m, b);

           d1 = sqrt((turningPoints(1,1,end) - intersection(1,1,end-1))^2 + (turningPoints(1,2,end) - intersection(1,2,end-1))^2);
           d2 = sqrt((turningPoints(1,1,end) - intersection(1,1,end))^2 + (turningPoints(1,2,end) - intersection(1,2,end))^2);

           if (d1 < d2) % if d1 is closer than d2, d1 is our intersection
               turningPoints = [turningPoints; intersection(:,:,end-1)];
           else % d2 is closer than d1
               turningPoints = [turningPoints; intersection(:,:,end)];
           end

       end
   elseif (cosAngle > 0)
       if (sinAngle < 0)     % if we are moving right AND down



           % start looking for first intersection

           %check the mirror directly below
           intersection = cl_intersection(V - 1, L, m, b);

           % if there was no intersection, check directly to the right
           if (~isreal(intersection(:,:,1)) && ~isreal(intersection(:,:,2)))
                intersection = cl_intersection(V, L + 1, m, b);
           end

           %check the remaining possible mirrors
           if (~isreal(intersection(:,:,1)) && ~isreal(intersection(:,:,2)))
               for i = 1:1:20
                    intersection = cl_intersection(V - 1, L + i, m, b);
                    if (~isreal(intersection(:,:,1)))
                        if (~isreal(intersection(:,:,2)))
                            intersection = cl_intersection(V - i, L + 1, m, b);
                            continue;
                        end
                    end

                   d1 = sqrt((turningPoints(1,1,end) - intersection(1,1,end-1))^2 + (turningPoints(1,2,end) - intersection(1,2,end-1))^2);
                   d2 = sqrt((turningPoints(1,1,end) - intersection(1,1,end))^2 + (turningPoints(1,2,end) - intersection(1,2,end))^2);

                   if (d1 < d2) % if d1 is closer than d2, d1 is our intersection
                       turningPoints = [turningPoints; intersection(:,:,end-1)];
                   else % d2 is closer than d1
                       turningPoints = [turningPoints; intersection(:,:,end)];
                   end
                   break;
               end
           else
                                  d1 = sqrt((turningPoints(end, 1, :) - intersection(1,1,end-1))^2 + (turningPoints(end, 2, :) - intersection(1,2,end-1))^2);
                   d2 = sqrt((turningPoints(end, 1, :) - intersection(1,1,end))^2 + (turningPoints(end, 2, :) - intersection(1,2,end))^2);

                   if (d1 < d2) % if d1 is closer than d2, d1 is our intersection
                       turningPoints = [turningPoints; intersection(:,:,end-1)];
                   else % d2 is closer than d1
                       turningPoints = [turningPoints; intersection(:,:,end)];
                   end

           end



       elseif (sinAngle > 0) % if we are moving right AND up
           % start looking for first intersection

           %check the mirror directly above
           intersection = cl_intersection(V + 1, L, m, b);

           % if there was no intersection
           if (~isreal(intersection(:,:,1)) && ~isreal(intersection(:,:,2)))
                intersection = cl_intersection(V, L + 1, m, b);
           end

           %check the remaining possible mirrors
           if (~isreal(intersection(:,:,1)) && ~isreal(intersection(:,:,2)))
               for i = 1:1:20
                    intersection = cl_intersection(V + 1, L + 1 + i, m, b);
                    if (~isreal(intersection(:,:,1)))
                        if (~isreal(intersection(:,:,2)))
                            intersection = cl_intersection(V + 1 + i, L + 1, m, b);
                            continue;
                        end
                    end

                   d1 = sqrt((turningPoints(1,1,end) - intersection(1,1,end-1))^2 + (turningPoints(1,2,end) - intersection(1,2,end-1))^2);
                   d2 = sqrt((turningPoints(1,1,end) - intersection(1,1,end))^2 + (turningPoints(1,2,end) - intersection(1,2,end))^2);

                   if (d1 < d2) % if d1 is closer than d2, d1 is our intersection
                       turningPoints = [turningPoints; intersection(:,:,end-1)];
                   else % d2 is closer than d1
                       turningPoints = [turningPoints; intersection(:,:,end)];
                   end
                   break;
               end
           else
                                  d1 = sqrt((turningPoints(1,1,end) - intersection(1,1,end-1))^2 + (turningPoints(1,2,end) - intersection(1,2,end-1))^2);
                   d2 = sqrt((turningPoints(1,1,end) - intersection(1,1,end))^2 + (turningPoints(1,2,end) - intersection(1,2,end))^2);

                   if (d1 < d2) % if d1 is closer than d2, d1 is our intersection
                       turningPoints = [turningPoints; intersection(:,:,end-1)];
                   else % d2 is closer than d1
                       turningPoints = [turningPoints; intersection(:,:,end)];
                   end

           end

       else                  % we are not moving up or down (purely rightwards)

       end

   else
       if (sinAngle < 0)     % if we are moving purely downwards

       elseif (sinAngle > 0) % if we are moving purely upwards

       end

   end

   % calculate the derivative at the point the laser hit the mirror
   mirrorX = turningPoints(end,1,:);
   mirrorY = turningPoints(end,2,:);

   if (mirrorY > round(mirrorY)) %if we hit one of the top quadrants
       if(mirrorX > round(mirrorX)) %top right quadrant
           derivative = -(mirrorX - round(mirrorX))/sqrt((1/3)^2 - (mirrorX - round(mirrorX))^2);
           b = mirrorY - derivative * mirrorX % rearranging y = mx + b

           % choose x values either side of the x-value where our laser
           % hits the mirror (this is for plotting the tangent line)
           x1 = mirrorX + 0.05;
           x2 = mirrorX - 0.05;

           % calculate the corresponding y values for plotting the tangent
           % line
           y1 = derivative*x1 + b;
           y2 = derivative*x2 + b;

           %plot the tangent line
           line([x1, x2],[y1, y2], 'Color', [61/255 165/255 244/255], 'LineStyle', '--', 'LineWidth', 1);

           % 90 degree rotation
           transformationMatrix = [0 1;-1 0];

           % vector that is rotated 90 degrees (i.e. orthogonal to) the
           % tangent line (subtract mirrorX and mirrorY so we rotate around
           % origin, add them back for plotting in the next line)
%            orthogonalVector = transformationMatrix*[x2-mirrorX ; y2-mirrorY];
%            line([mirrorX, orthogonalVector(1)+mirrorX],[mirrorY, orthogonalVector(2)+mirrorY], 'Color', [61/255 165/255 244/255], 'LineStyle', '-', 'LineWidth', 2);



           nx1 = mirrorX + 0.35;
           nx2 = mirrorX - 0.35;

           nm = -1/derivative;
           nb = mirrorY - nm * mirrorX % rearranging y = mx + b - this b value is the b value for plotting our tangent line
           ny1 = nm*nx1 + nb;
           ny2 = nm*nx2 + nb;

           line([nx1, nx2],[ny1, ny2], 'Color', [61/255 165/255 244/255], 'LineWidth', 0.5);


           V = round(mirrorY);
           L = round(mirrorX);


           % the magnitude (i.e. positive value) of the angle between the incident ray, and the vector orthogonal to the tangent line
           % (the blue theta in my book)
           angleMag = pi/2 - abs(atan(derivative));

           % the new angle (i.e. the angle of the reflected ray)
           angle = atan(derivative) + pi/2 + (pi/2 - abs(atan(derivative)))

                      angleMag = abs(atan(nm) - atan(m));

           angle = atan(nm) + angleMag;
           globalAngle = angle;
           m = tan(angle); % our m value in y = mx + b
%            transformationMatrix = [cos(angle) -sin(angle); sin(angle) cos(angle)]
           b =  mirrorY - m*mirrorX; % rearrange y = mx + b
%            transformationMatrix = -1*[cos(atan(1/derivative)) sin(atan(1/derivative));-sin(atan(1/derivative)) cos(atan(1/derivative))];
%            newVector = transformationMatrix*[orthogonalVector(1)-mirrorX ; orthogonalVector(2)-mirrorY];
           line([0, mirrorX+3],[b,m*(mirrorX+3) + b], 'Color', [244/255 165/255 244/255], 'LineWidth', 0.5);
         xlim([-1 3])
         ylim([-1 3])


       else % we hit the top left quadrant

           derivative = -(mirrorX - round(mirrorX))/sqrt((1/3)^2 - (mirrorX - round(mirrorX))^2); % slope of the line that runs tangent to the point we hit
           b = mirrorY - derivative * mirrorX % rearranging y = mx + b - this b value is the b value for plotting our tangent line

           % choose x values either side of the x-value where our laser
           % hits the mirror (this is for plotting the tangent line)
           x1 = mirrorX + 0.05;
           x2 = mirrorX - 0.05;
           V = round(mirrorY);
           L = round(mirrorX);

           % calculate the corresponding y values for plotting the tangent
           % line
           y1 = derivative*x1 + b;
           y2 = derivative*x2 + b;

           %plot the tangent line
           line([x1, x2],[y1, y2], 'Color', [61/255 165/255 244/255],'LineWidth', 0.5);

           % 90 degree rotation
           transformationMatrix = [0 1;-1 0];

           % vector that is rotated 90 degrees (i.e. orthogonal to) the
           % tangent line (subtract mirrorX and mirrorY so we rotate around
           % origin, add them back for plotting in the next line)
 orthogonalVector = transformationMatrix*[x2-mirrorX ; y2-mirrorY];
line([mirrorX, orthogonalVector(1)+mirrorX],[mirrorY, orthogonalVector(2)+mirrorY], 'Color', [255/255 0/255 0/255], 'LineWidth', 0.5);




           nx1 = mirrorX + 0.35;
           nx2 = mirrorX - 0.35;

           nm = -1/derivative;
           nb = mirrorY - nm * mirrorX % rearranging y = mx + b - this b value is the b value for plotting our tangent line
           ny1 = nm*nx1 + nb;
           ny2 = nm*nx2 + nb;

           line([nx1, nx2],[ny1, ny2], 'Color', [61/255 165/255 244/255], 'LineWidth', 0.5);


           V = round(mirrorY);
           L = round(mirrorX);
%            angleMag = abs((atan(m) + pi/2 - atan(derivative)));

           angle = (atan(m) + pi/2 - atan(derivative));
           if (angle > 0)
              globalAngle = pi - atan(m);
           end
%            angle = (pi/2 + 2*abs(atan(derivative)))
           m = tan(atan(nm) + pi - angle);
%            transformationMatrix = [cos(angle) -sin(angle); sin(angle) cos(angle)]
           b =  mirrorY - m*mirrorX; % rearrange y = mx + b
%            transformationMatrix = -1*[cos(atan(1/derivative)) sin(atan(1/derivative));-sin(atan(1/derivative)) cos(atan(1/derivative))];
%            newVector = transformationMatrix*[orthogonalVector(1)-mirrorX ; orthogonalVector(2)-mirrorY];
           line([0, mirrorX+3],[b,m*(mirrorX+3) + b], 'Color', [244/255 165/255 244/255], 'LineWidth', 0.5);




%                       m = 1/tan (angle);
%            transformationMatrix = [cos(angle) -sin(angle); sin(angle) cos(angle)]
%            b =  mirrorY - m*mirrorX; % rearrange y = mx + b
%            transformationMatrix = -1*[cos(atan(1/derivative)) sin(atan(1/derivative));-sin(atan(1/derivative)) cos(atan(1/derivative))];
%            newVector = transformationMatrix*[orthogonalVector(1)-mirrorX ; orthogonalVector(2)-mirrorY];
%            line([0, mirrorX+3],[b,m*(mirrorX+3) + b], 'Color', [244/255 165/255 244/255], 'LineWidth', 0.5);
       end



   else % we hit one of the bottom quadrants
       if (mirrorX > round(mirrorX)) % we hit the bottom right quadrant
           derivative = (mirrorX - round(mirrorX))/sqrt((1/3)^2 - (mirrorX - round(mirrorX))^2);
           b = mirrorY - derivative * mirrorX % rearranging y = mx + b

           % choose x values either side of the x-value where our laser
           % hits the mirror (this is for plotting the tangent line)
           x1 = mirrorX + 0.35;
           x2 = mirrorX - 0.35;

           % calculate the corresponding y values for plotting the tangent
           % line
           y1 = derivative*x1 + b;
           y2 = derivative*x2 + b;

           %plot the tangent line
           line([x1, x2],[y1, y2], 'Color', [61/255 165/255 244/255], 'LineWidth', 0.5);

           % 90 degree rotation
           transformationMatrix = [0 1;-1 0];

           % vector that is rotated 90 degrees (i.e. orthogonal to) the
           % tangent line (subtract mirrorX and mirrorY so we rotate around
           % origin, add them back for plotting in the next line)
%            orthogonalVector = -1*transformationMatrix*[x2-mirrorX ; y2-mirrorY];
%            line([mirrorX, orthogonalVector(1)+mirrorX],[mirrorY, orthogonalVector(2)+mirrorY], 'Color', [61/255 165/255 244/255], 'LineStyle', '-', 'LineWidth', 2);
            V = round(mirrorY);
            L = round(mirrorX);

           nx1 = mirrorX + 0.35;
           nx2 = mirrorX - 0.35;

           nm = -1/derivative;
           nb = mirrorY - nm * mirrorX % rearranging y = mx + b - this b value is the b value for plotting our tangent line

           ny1 = nm*nx1 + nb;
           ny2 = nm*nx2 + nb;

           line([nx1, nx2],[ny1, ny2], 'Color', [61/255 165/255 244/255], 'LineWidth', 0.5);

           % the magnitude (i.e. positive value) of the angle between the incident ray, and the vector orthogonal to the tangent line
           % (the blue theta in my book)
           angleMag = abs(atan(nm) - atan(m));

           % the new angle (i.e. the angle of the reflected ray)
%            angle = -(atan(m) + pi/2 - atan(derivative));
             angle = atan(nm) - angleMag;
           if (angle < 0)
              globalAngle = atan(m);
           end
%            angle = (pi/2 + 2*abs(atan(derivative)))
           m = tan(angle);
%            transformationMatrix = [cos(angle) -sin(angle); sin(angle) cos(angle)]
           b =  mirrorY - m*mirrorX; % rearrange y = mx + b
           globalAngle = angle;

%            transformationMatrix = [cos(angle) -sin(angle); sin(angle) cos(angle)]
%            transformationMatrix = -1*[cos(atan(1/derivative)) sin(atan(1/derivative));-sin(atan(1/derivative)) cos(atan(1/derivative))];
%            newVector = transformationMatrix*[orthogonalVector(1)-mirrorX ; orthogonalVector(2)-mirrorY];
           line([0, mirrorX+3],[b,m*(mirrorX+3) + b], 'Color', [244/255 165/255 244/255], 'LineWidth', 0.5);
         xlim([-1 3])
       end

   end
end