clear; clc; close all;
% Robot Position
R = [0;0;0]; % Robot Starting Postion
R0 = R.*[1;1;0]; % Robot Base Position
% Winch Radius: 
r = 0.005; % m
% Magnitude of Desired Force:
F = 20; % N

% Anchor point locations
% AnchorCircleRadius = 0.4572; % m (18in radius)
% AnchorCircleRadius = 0.6096; % m (24in radius)
AnchorCircleRadius = 0.6;
A1 = AnchorCircleRadius*[cosd(90); sind(90); 0]; % Top Anchor
A2 = AnchorCircleRadius*[cosd(210); sind(210);0]; % Left Anchor
A3 = AnchorCircleRadius*[cosd(-30); sind(-30);0]; % Right Anchor

X = -0.1524:0.01:0.1524;
Y = -0.1524:0.01:0.1524;
Z = 0;

TorqueSurface = zeros(length(Y), length(X));

hold on
plot([A1(1),A3(1),A2(1),A1(1)],[A1(2),A3(2),A2(2),A1(2)],'bo-','LineWidth',2);
plot([R(1)],R(2))


for i=1:length(X)
    for j=1:length(Y)
        R = [X(i);Y(j);Z];
        plot(R(1), R(2), '.', 'Color', [0.6 0.6 0.6]);

        % User applied force
        rad(i) = ((i-1)*0.01)*pi();
        Fx(i) = cos(rad(i))*F;
        Fy(j) = sin(rad(i))*F;
        desiredForce(:,i,j) = [Fx(i);Fy(j);0];

        V1 = A1-R; 
        u1 = V1/norm(V1);
        V2 = A2-R; 
        u2 = V2/norm(V2); 
        V3 = A3-R;
        u3 = V3/norm(V3); 
        c1 = u1.*(1+[1;1;0]); 
        c2 = u2.*(1+[1;1;0]); 
        c3 = u3.*(1+[1;1;0]); 
        c4 = [0;0;1]; 
        C = [c1 c2 c3 c4];

        wireForces(:,i,j) = lsqnonneg(C,desiredForce(:,i,j));
        torques = wireForces(:,i,j).*r;
        torque1 = torques(1);
        torque2 = torques(2);
        torque3 = torques(3);
        totaltorque = torque1+torque2+torque3;
        TorqueSurface(j,i) = norm(totaltorque);

        maxTension = max(wireForces(:,i,j));
        minTension = min(wireForces(:,i,j));
        maxTorque = r.*maxTension;

    end
end
V = 1; % m/s

RPM = (V*60)/(pi()*r);

[Xgrid, Ygrid] = meshgrid(X, Y);
figure(2);
surf(Xgrid, Ygrid, TorqueSurface);
xlabel('X (m)');
ylabel('Y (m)');
zlabel('Summed Torque (Nm)');