function Simulation()
[ps1, ps2, ps3, ps4] = plotlySetup();

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

% Start the 3D Visual window
agx('visual', 1)

while(1)
  ps1.open();
  ps2.open();
  ps3.open();
  ps4.open();
  
  simulationFile = 'crane.agxLua';
  t = 0;
  timeStep = 1/60.0;
  luaControl = 0;
  
  initInput = luaControl;
  [~, ~, ~, ~] = 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 speedPelare
  % 2 speedLyftarm
  % 3 speedGrapple
  % 4 speedBaseRotator
  % 5 speedGrappleRotator
  % 6 speedUtskjutMotor
  
  %Lengths used to find end-affectors position, e.g booms , measured using SpaceClaim
  %L1 = 1.987502; %old base. Should there be a CoM offset here aswell?
  %Probably works because offset is so small.
  % L1 = 1.987502 + 0.10109; %new base. Boom length + CoM offset
  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 0.0];
  W = diag(ones(4, 1));
  w1 = 2.0;
  w2 = 1.5;
  w3 = 1.5;
  w4 = 1.0;
  logRand = 0;
  logsDone = [];
  logsLeft = [1 2 3 4];
  nrLogs = 4;
  logFound = 0; logAngleFound = 0; grabLog = 0; logGrabbed = 0; findCarriage = 0;
  carriageFound = 0; carriageAngleFound = 0; dropLog = 0; logDropped = 0; positionReset = 0;
  
  carriageOffset = 0;
  
  window_size = 5;
  pelareMotorForce_recent = zeros(window_size + 1, 1);
  grappleMotorForce_recent = zeros(window_size + 1, 1);
  baseRotatorMotorForce_recent = zeros(window_size + 1, 1);
  
  sumWork = 0;
  
  tic
  disp('Running simulation...')
  while (logsLeft)
    t = t + timeStep;
    
    if (logGrabbed)
      input(7) = 5;
    else
      input(7) = logRand;
    end
    
    %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 0.0];
      
      continue;
    elseif (t < 0.5)
      input = [0.0 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);
    logList(1, :) = stepOutput(15:20);
    logList(2, :) = stepOutput(21:26);
    logList(3, :) = stepOutput(27:32);
    logList(4, :) = stepOutput(33:38);
    simTime = stepOutput(39);
    
    [sumWork, pelareMotorForce_recent, ...
      grappleMotorForce_recent, baseRotatorMotorForce_recent] = ...
      forceAndWork(stepOutput, sumWork, t, ps1, ps2, ps3, ps4, timeStep, window_size, ...
      pelareMotorForce_recent, grappleMotorForce_recent, baseRotatorMotorForce_recent);
    
    %Select a new log from list at random
    if (logRand == 0)
      logsLeft = setdiff([1 2 3 4], logsDone);
      logRand = logsLeft(floor(nrLogs*rand()) + 1);
    end
    currLogPos = logList(logRand, 1:3);
    currLogVec = logList(logRand, 4:6);
    currLogVec = currLogVec(1:2)./norm(currLogVec(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 log, grab log, center above carriage,
    %drop log. Next log.
    
    %Determine current target.
    if logFound == 0
      target = currLogPos + [0, 0, 3];
      grappleTurnAngle = acos(currLogVec*[sin(angleGrapple - theta1) cos(angleGrapple - theta1)]') - pi/2;
      
      if norm(target - p) < 0.10
        logFound = 1;
        currLogPos_ground = currLogPos;
      end
      
    elseif logAngleFound == 0
      target = currLogPos + [0, 0, 3];
      grappleTurnAngle = acos(currLogVec*[sin(angleGrapple - theta1) cos(angleGrapple - theta1)]') - pi/2;
      
      if abs(grappleTurnAngle) < 0.10
        logAngleFound = 1;
      end
      
    elseif (logFound == 1 && logAngleFound == 1) && (grabLog == 0) && logGrabbed ~= 1
      target = currLogPos_ground + [0, 0, 1.15];
      
      if norm(target - p) < 0.10
        grabLog = 1;
        grabEnd = t + 1;
      end
      
    elseif (grabLog == 1)
      input = [0.0 0.0 -0.2 0.0 0.0 0.0 0.0];
      
      if t > grabEnd
        grabLog = 0;
        logGrabbed = 1;
      else
        continue;
      end
      
    elseif (logGrabbed == 1 && findCarriage == 0)
      target = currLogPos_ground + [0, 0, 3];
      
      if norm(target - p) < 0.20
        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) && dropLog == 0 && logDropped ~= 1 && positionReset ~= 1
      target = carriagePos + [carriageOffset, 0, 2.0];
      
      if norm(target - p) < 0.15
        dropLog = 1;
        dropEnd = t + 2;
      end
      
    elseif (dropLog == 1)
      
      if t > dropEnd - 1
        input = [0.0 0.0 0.13 0.0 0.0 0.0 0.0];
      else
        input = [0.0 0.0 0.0 0.0 0.0 0.0 0.0];
      end
      
      if t > dropEnd
        dropLog = 0;
        logDropped = 1;
      else
        continue;
      end
    elseif (logDropped == 1)
      target = carriagePos + [0, 0, 3];
      
      if norm(target - p) < 0.20
        logDropped = 0;
        positionReset = 1;
      end
      
    elseif (positionReset == 1)
      logFound = 0; logAngleFound = 0; grabLog = 0; logGrabbed = 0; findCarriage = 0;
      carriageFound = 0; carriageAngleFound = 0; dropLog = 0; logDropped = 0; positionReset = 0;
      
      logsDone = [logsDone logRand];
      logsLeft = setdiff([1 2 3 4], logsDone)
      logRand = 0;
      nrLogs = nrLogs - 1;
    end
    
    %Straight line from current end-affector position to target position.
    p_dot = target - p;
    
    min_speed = 0.4;
    max_speed = 1.2;
    
    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)];
    
    %pause(0.01)
  end
  
  
  input = [0.0 0.0 0.0 0.0 0.0 0.0 0.0];
  
  for i = 1:100
    [stepOutput] = agx('step', input);
  end
  
  ps1.close();
  ps2.close();
  ps3.close();
  ps4.close();
  
  disp('Simulation done...')
  t
  simTime
  
  toc
  
  pause(3)
end
end



function [sumWork, pelareMotorForce_recent, ...
  grappleMotorForce_recent, baseRotatorMotorForce_recent] = ...
  forceAndWork(stepOutput, sumWork, t, ps1, ps2, ps3, ps4, timeStep, window_size, ...
  pelareMotorForce_recent, grappleMotorForce_recent, baseRotatorMotorForce_recent)

% New way of getting force (hopefully right...), getCurrentForce(...).
simTime = stepOutput(39);
baseRotatorMotorSpeed = abs(stepOutput(40));
pelareMotorSpeed = abs(stepOutput(41));
% lyftarmMotorSpeed = stepOutput(42);
% utskjutMotorSpeed = stepOutput(43);
% rotatorMotorSpeed = stepOutput(44);
grappleMotorSpeed = abs(stepOutput(45));

baseRadii = 0.15405/2; %154.05 mm
baseRotatorMotorForce = abs(stepOutput(46))/baseRadii;
pelareMotorForce = abs(stepOutput(47));
% lyftarmMotorForce = stepOutput(48);
% utskjutMotorForce = stepOutput(49);
% rotatorRadii = ...;
% rotatorMotorForce = stepOutput(50)/rotatorRadii;
grappleMotorForce = abs(stepOutput(51));

%Force exp moving avg
pelareMotorForce_recent(1:(end - 1)) = pelareMotorForce_recent(2:end);
pelareMotorForce_recent(end) = norm(pelareMotorForce);
pelareMotorForce_exp = tsmovavg(pelareMotorForce_recent, 'e', window_size, 1);

grappleMotorForce_recent(1:(end - 1)) = grappleMotorForce_recent(2:end);
grappleMotorForce_recent(end) = norm(grappleMotorForce);
grappleMotorForce_exp = tsmovavg(grappleMotorForce_recent, 'e', window_size, 1);

%Torque exp moving avg
baseRotatorMotorForce_recent(1:(end - 1)) = baseRotatorMotorForce_recent(2:end);
baseRotatorMotorForce_recent(end) = norm(baseRotatorMotorForce);
baseRotatorMotorForce_exp = tsmovavg(baseRotatorMotorForce_recent, 'e', window_size, 1);

%Work specified motors
sumWork = sumWork + pelareMotorForce_exp(end)*abs(pelareMotorSpeed)*timeStep + ...
  grappleMotorForce_exp(end)*abs(grappleMotorSpeed)*timeStep + ...
  baseRotatorMotorForce_exp(end)*abs(baseRotatorMotorSpeed)*timeStep;

if (mod(t, 10*timeStep) < timeStep)
  trace1.x = simTime;
  trace1.y = pelareMotorForce_exp(end);
  trace2.x = simTime;
  trace2.y = baseRotatorMotorForce_exp(end);
  trace3.x = simTime;
  trace3.y = grappleMotorForce_exp(end);
  
  trace4.x = simTime;
  trace4.y = sumWork;
  
  ps1.write(trace1);
  ps2.write(trace2);
  ps3.write(trace3);
  ps4.write(trace4);
end
end

function [ps1, ps2, ps3, ps4] = plotlySetup()
plotlysetup('mapo0024','67jed2rt14','stream_ids',{'tzahq3k9xf', 'dqlechx3h4', '4ij03yz1cm', '47rr8f0jpx', 'e6tgbdjcbx', 'one4y34pye', 'k4i52lipeo'})
% plotlysetup('watler','i85m9z9d1f','stream_ids',{'0e0ndgclrn', 'csl7oex4cl', 'uudpatm1ii', '2nkt606rxa', 'c3sf4fx38l', 'mvc1l3glf5', 'g4pme43uut'});
my_credentials = loadplotlycredentials;

fig = plotlyfig('visible','off');

args.layout.showlegend = false;
args.layout.height = 300;
args.layout.width = 1200;
args.layout.margin.l = 50;
args.layout.margin.r = 20;
args.layout.margin.b = 20;
args.layout.margin.t = 20;

maxpoints = 1000;

fig.data{1}.type = 'scatter';
fig.data{1}.mode = 'lines';
fig.data{1}.x = [];
fig.data{1}.y = [];
fig.data{1}.line.width = 2;
fig.data{1}.line.shape = 'spline';
fig.data{1}.line.color = 'rgba(27, 158, 119, 1.0)';
fig.data{1}.stream.token = my_credentials.stream_ids{end-3};
fig.data{1}.stream.maxpoints = maxpoints;
fig.data{1}.autorange = true;
fig.data{1}.name = '1st Cylinder';
fig.data{1}.xaxis = 'x1';
fig.data{1}.yaxis = 'y1';

fig.data{2}.type = 'scatter';
fig.data{2}.mode = 'lines';
fig.data{2}.x = [];
fig.data{2}.y = [];
fig.data{2}.line.width = 2;
fig.data{2}.line.shape = 'spline';
fig.data{2}.line.color = 'rgba(217, 95, 2, 1.0)';
fig.data{2}.stream.token = my_credentials.stream_ids{end-2};
fig.data{2}.stream.maxpoints = maxpoints;
fig.data{2}.autorange = true;
fig.data{2}.name = 'Slew';
fig.data{2}.xaxis = 'x1';
fig.data{2}.yaxis = 'y1';

fig.data{3}.type = 'scatter';
fig.data{3}.mode = 'lines';
fig.data{3}.x = [];
fig.data{3}.y = [];
fig.data{3}.line.width = 2;
fig.data{3}.line.shape = 'spline';
fig.data{3}.line.color = 'rgba(117, 112, 179, 1.0)';
fig.data{3}.stream.token = my_credentials.stream_ids{end-1};
fig.data{3}.stream.maxpoints = maxpoints;
fig.data{3}.autorange = true;
fig.data{3}.name = 'Gripper';
fig.data{3}.xaxis = 'x1';
fig.data{3}.yaxis = 'y1';

fig.data{4}.type = 'scatter';
fig.data{4}.mode = 'lines';
fig.data{4}.line.width = 2;
fig.data{4}.line.color = 'rgba(0, 20, 250, 0.5)';
fig.data{4}.line.shape = 'spline';
fig.data{4}.x = [];
fig.data{4}.y = [];
fig.data{4}.stream.token = my_credentials.stream_ids{end};
fig.data{4}.stream.maxpoints = maxpoints;
fig.data{4}.name = 'Total work';
fig.data{4}.autorange = true;
fig.data{4}.xaxis = 'x2';
fig.data{4}.yaxis = 'y2';

%---layout and plot options---%
% args.layout.title = 'Test stream 1';
% args.layout.xaxis1.title = 'Time';
args.layout.xaxis1.showgrid = true;
args.layout.xaxis1.showline = false;
args.layout.xaxis1.mirror = false;
args.layout.xaxis1.nticks = 8;
args.layout.xaxis1.anchor = 'y1';
args.layout.yaxis1.title = 'Force [N]';
args.layout.yaxis1.anchor = 'x1';
% args.layout.yaxis1.range = [-1 1];
args.layout.yaxis1.nticks = 4;
args.layout.yaxis1.showline = true;
args.layout.yaxis1.mirror = false;
args.layout.yaxis1.zeroline = true;
args.layout.yaxis1.zerolinewidth = 2;
args.layout.yaxis1.showgrid = true;
args.layout.yaxis1.domain = [0.55 1];

args.layout.yaxis2.title = 'Work [J]';
args.layout.xaxis2.title = 'Time [s]';
args.layout.xaxis2.showline = true;
args.layout.xaxis2.zeroline = false;
args.layout.xaxis2.mirror = false;
args.layout.xaxis2.nticks = 8;
args.layout.xaxis2.anchor = 'y2';
args.layout.xaxis2.linewidth = 2;
args.layout.yaxis2.showline = true;
args.layout.yaxis2.linewidth = 2;
args.layout.yaxis2.mirror = false;
args.layout.yaxis2.nticks = 4;
args.layout.yaxis2.zeroline = false;
args.layout.yaxis2.domain = [0.05 0.45];
args.layout.yaxis2.anchor = 'x2';
% args.layout.yaxis2.range = [-1 1];

args.filename = 'Simulation_Stream';
args.fileopt = 'overwrite';
resp = plotly(fig.data,args);
resp.url

%create plotlystream objects
sigstream_time1 = plotlystream(fig.data{1}.stream.token);
sigstream_time2 = plotlystream(fig.data{2}.stream.token);
sigstream_time3 = plotlystream(fig.data{3}.stream.token);
sigstream_time4 = plotlystream(fig.data{4}.stream.token);

ps1 = sigstream_time1;
ps2 = sigstream_time2;
ps3 = sigstream_time3;
ps4 = sigstream_time4;

end
