function GrippingBoxes()
% Start by resetting the AgX simulation
agx('reset')

% Start the 3D Visual window
agx('visual', 1)
simulationFile = 'GrippingBoxes.py';
t = 0;
timeStep = 0.01;

initInput = [];
[~, ~, ~, ~] = agx('load', simulationFile, initInput, timeStep);

%Angular speed -> Hydraulic cylinder speed. Found through point fitting.
f_AngSpd_t2 = @(x) -0.11893.*x.*x + 0.020272*x + 0.3737;
f_AngSpd_t3 = @(x) -0.005244.*x.*x - 0.049929*x + 0.40602;

%Weight functions
d_min = 0;
d_max = 1.95;
delta_d = 0.1;
f_d = @(x) 1./(1 + exp(-100*(x - (d_min + delta_d)))) + 1./(1 + exp(100*(x - (d_max - delta_d)))) - 0.9;

t2_min = deg2rad(-10);
t2_max = deg2rad(55);
delta_t2 = 0.1;
f_t2 = @(x) 1./(1 + exp(-100*(x - (t2_min + delta_t2)))) + 1./(1 + exp(100*(x - (t2_max - delta_t2)))) - 0.9;

t3_min = deg2rad(-145);
t3_max = deg2rad(-5);
delta_t3 = 0.2;
f_t3 = @(x) 1./(1 + exp(-100*(x - (t3_min + delta_t3)))) + 1./(1 + exp(100*(x - (t3_max - delta_t3)))) - 0.9;

% --- input order ---
% 1 speedInnerBoomMotor
% 2 speedOuterBoomMotor
% 3 speedGrappleMotor
% 4 speedBaseRotator
% 5 speedGrappleRotator
% 6 speedExtensionMotor

%Lengths used to find end-affectors position, e.g booms , measured using SpaceClaim
L1 = 1.987502 + 0.26835; %new base. Boom length + CoM offset
L2 = 3.8887027;
L3 = 2.3315488;
D0 = 0.116459;
D1 = 0.1734;
D2 = 0.513883;

input = [0.0 0.0 0.0 0.0 0.0 0.0];
W = diag(ones(4, 1));
w1 = 2.0;
w2 = 1.5;
w3 = 1.5;
w4 = 1.0;
boxFound = 0; boxAngleFound = 0; grabBox = 0; boxGrabbed = 0; findCarriage = 0;
carriageFound = 0; carriageAngleFound = 0; dropBox = 0; boxDropped = 0; positionReset = 0;

grabEnd = 100000000; 

carriageOffset = 0;

tic
disp('Running simulation...')
while (~positionReset)
  t = t + timeStep;
  
  %step AgX
  [stepOutput] = agx('step', input);
  
  %Make sure no motors are at their range limits
  if (t < 0.3)
    input = [0.2 0.2 0.25 0.0 0.0 0.4];
    
    continue;
  elseif (t < 0.5)
    input = [0.0 0.0 0.0 0.0 0.0 0.0];
    
    continue;
  end
  
  %get output
  theta1 = stepOutput(1);
  theta2 = stepOutput(2) - 0.0269; %angle offset
  theta3 = stepOutput(3) - 0.1208; %angle offset
  d = stepOutput(4);
  base = stepOutput(5:7);
  angleGrapple = stepOutput(8);
  carriagePos = stepOutput(9:11);
  carriageVec = stepOutput(12:14);
  carriageVec = carriageVec(1:2);
  box = stepOutput(15:20);
  simTime = stepOutput(21);
  
  currBoxPos = box(1:3);
  currBoxVec = box(4:6);
  currBoxVec = currBoxVec(1:2)./norm(currBoxVec(1:2));
  
  %End-affector position
  r = L2*cos(theta2) - D1*sin(theta2) + (L3 + d)*cos(theta2 + theta3) - D2*sin(theta2 + theta3);
  h = L1 + L2*sin(theta2) + D1*cos(theta2) + (L3 + d)*sin(theta2 + theta3) + D2*cos(theta2 + theta3);
  
  %Partial end-affector position
  r_start = [base(1) - D0*cos(theta1), base(2) - D0*sin(theta1), base(3)];
  r_end = r_start + [(r - D0)*cos(theta1), (r - D0)*sin(theta1), 0];
  h_start = r_end;
  h_end = r_end + [0, 0, h];
  p_start = r_start;
  p = r_start + [(r - D0)*cos(theta1), (r - D0)*sin(theta1), h];
  
  %Terms in Jacobian
  A = (L3 + d)*sin(theta2 + theta3) + D2*cos(theta2 + theta3); %h - h_p3
  B = L2*sin(theta2) + D1*cos(theta2) + A; %h - L_1
  C = L2*cos(theta2) - D1*sin(theta2); % r_p3
  D = C + (L3 + d)*cos(theta2 + theta3) - D2*sin(theta2 + theta3); %r
  
  %Jacobian
  J = [-sin(theta1)*D -cos(theta1)*B -cos(theta1)*A cos(theta1);
    cos(theta1)*D -sin(theta1)*B -sin(theta1)*A sin(theta1);
    0 D (D - C) sin(theta2 - theta3)];
  
  grappleTurnAngle = 0;
  
  %One "load cycle". Center above box, grab box, center above carriage,
  %drop box.
  
  %Determine current target.
  if boxFound == 0
    target = currBoxPos + [0, 0, 3];
    grappleTurnAngle = acos(currBoxVec*[sin(angleGrapple - theta1) cos(angleGrapple - theta1)]') - pi/2;
    
    if norm(target - p) < 0.1
      boxFound = 1;
      currBoxPos_ground = currBoxPos;
    end
    
  elseif boxAngleFound == 0
    target = currBoxPos + [0, 0, 3];
    grappleTurnAngle = acos(currBoxVec*[sin(angleGrapple - theta1) cos(angleGrapple - theta1)]') - pi/2;
    
    if abs(grappleTurnAngle) < 0.1
      boxAngleFound = 1;
    end
    
  elseif (boxFound == 1 && boxAngleFound == 1) && (grabBox == 0) && boxGrabbed ~= 1
    target = currBoxPos_ground + [0, 0, 1.5];
    
    if norm(target - p) < 0.01
      grabBox = 1;
      grabEnd = t + 1;
    end
    
  elseif (grabBox == 1)
    input = [0.0 0.0 -0.25 0.0 0.0 0.0];
    
    if t > grabEnd
      grabBox = 0;
      boxGrabbed = 1;
    else
      continue;
    end
    
  elseif (boxGrabbed == 1 && findCarriage == 0)
    target = currBoxPos_ground + [0, 0, 3];
    
    if norm(target - p) < 0.10
      findCarriage = 1;
    end
    
  elseif (findCarriage == 1 && carriageFound == 0)
    target = carriagePos + [carriageOffset, 0, 3.5];
    grappleTurnAngle = acos(carriageVec*[sin(angleGrapple - theta1) cos(angleGrapple - theta1)]') - pi/2;
    
    if norm(target - p) < 0.10
      carriageFound = 1;
    end
    
  elseif carriageAngleFound == 0
    target = carriagePos + [carriageOffset, 0, 3.5];
    grappleTurnAngle = acos(carriageVec*[sin(angleGrapple - theta1) cos(angleGrapple - theta1)]') - pi/2;
    
    if abs(grappleTurnAngle) < 0.05
      carriageAngleFound = 1;
    end
    
  elseif (carriageFound == 1 && carriageAngleFound == 1) && dropBox == 0 && boxDropped ~= 1 && positionReset ~= 1
    target = carriagePos + [carriageOffset, 0, 2.0];
    
    if norm(target - p) < 0.10
      dropBox = 1;
      dropEnd = t + 2;
    end
    
  elseif (dropBox == 1)
    
    if t > dropEnd - 1
      input = [0.0 0.0 0.13 0.0 0.0 0.0];
    else
      input = [0.0 0.0 0.0 0.0 0.0 0.0];
    end
    
    if t > dropEnd
      dropBox = 0;
      boxDropped = 1;
    else
      continue;
    end
  elseif (boxDropped == 1)
    target = carriagePos + [0, 0, 3];
    
    if norm(target - p) < 0.10
      boxDropped = 0;
      positionReset = 1;
    end
  end
  
  %Straight line from current end-affector position to target position.
  p_dot = target - p;
  
  min_speed = 0.3;
  max_speed = 1.0;
  
  if (norm(p_dot) > max_speed)
    p_dot = max_speed*p_dot./norm(p_dot);
  elseif ((norm(p_dot) < min_speed))
    p_dot = min_speed*p_dot./norm(p_dot);
  end
  
  %Set weigths and solve the system.
  W(1, 1) = w1;
  W(2, 2) = w2*f_t2(theta2);
  W(3, 3) = w3*f_t3(theta3);
  W(4, 4) = w4*f_d(d);
  J_inv = (W*J')*inv(J*W*J');
  theta_dot = J_inv*p_dot';
  
  %Actuator speeds, both prismatics/hyrdaulics and hinges. Input for next step.
  input = [f_AngSpd_t2(theta2)*theta_dot(2) -f_AngSpd_t3(theta3)*theta_dot(3) 0 theta_dot(1) grappleTurnAngle theta_dot(4)];
  
  if t > grabEnd
    input(3) = -0.25;
  end
  
  
  %pause(0.01)
end


input = [0.0 0.0 0.0 0.0 0.0 0.0];

for i = 1:100
  [stepOutput] = agx('step', input);
end

disp('Simulation done...')
t
simTime

toc

pause(3)
end
