function MonitorControl()
close all

  function log1Fcn(varargin)
    if (~grabLog && ~logGrabbed && logRand ~= 1)
      logRand = 1;
      logFound = 0; logAngleFound = 0; grabLog = 0; logGrabbed = 0; findCarriage = 0;
      carriageFound = 0; carriageAngleFound = 0; dropLog = 0; logDropped = 0; positionReset = 0;
    end
  end

  function log2Fcn(varargin)
    if (~grabLog && ~logGrabbed && logRand ~= 2)
      logRand = 2;
      logFound = 0; logAngleFound = 0; grabLog = 0; logGrabbed = 0; findCarriage = 0;
      carriageFound = 0; carriageAngleFound = 0; dropLog = 0; logDropped = 0; positionReset = 0;
    end
  end

  function log3Fcn(varargin)
    if (~grabLog && ~logGrabbed && logRand ~= 3)
      logRand = 3;
      logFound = 0; logAngleFound = 0; grabLog = 0; logGrabbed = 0; findCarriage = 0;
      carriageFound = 0; carriageAngleFound = 0; dropLog = 0; logDropped = 0; positionReset = 0;
    end
  end

  function log4Fcn(varargin)
    if (~grabLog && ~logGrabbed && logRand ~= 4)
      logRand = 4;
      logFound = 0; logAngleFound = 0; grabLog = 0; logGrabbed = 0; findCarriage = 0;
      carriageFound = 0; carriageAngleFound = 0; dropLog = 0; logDropped = 0; positionReset = 0;
    end
  end

  function dropLogDown(varargin)
    if (logGrabbed && ~logDropped && ~carriageFound)
      input = [0.0 0.0 0.0 0.0 0.0 0.0 0.0];
      dropTime = 0;
      while (dropTime < 250)
        dropTime = dropTime + 1;
        if (dropTime > 150)
          input = [0.0 0.0 0.2 0.0 0.0 0.0 0.0];
        end
        
        [stepOutput] = agx('step', input);
      end
      logFound = 0; logAngleFound = 0; grabLog = 0; logGrabbed = 0; findCarriage = 0;
      carriageFound = 0; carriageAngleFound = 0; dropLog = 0; logDropped = 0; positionReset = 0;
      logRand = logsLeft(floor(nrLogs*rand()) + 1);
    end
    
  end


  function btnSelection(~, eventdata, ~)
    switch get(eventdata.NewValue,'Tag') % Get Tag of selected object.
      case 'Center'
        carriageOffset = 0;
      case 'Left'
        carriageOffset = 0.7;
      case 'Right'
        carriageOffset = -0.7;
    end
  end

%read image
fullColorPlot = imread('crane_terrain.png');
partialColorPlot = fliplr(fullColorPlot(1:128, 1:128, 1));

% [Gx, Gy] = imgradientxy(partialColorPlot);
% 
% normGrad = sqrt(Gx.^2 + Gy.^2);
% gradPlot = Gx;
% gradPlot(normGrad > 10) = normGrad(normGrad > 10);
% gradPlot = abs(gradPlot);
% gradPlot(gradPlot < 0.12) = gradPlot(gradPlot < 0.12)*3 + 0.0005;
% gradPlot(gradPlot < 0.25) = gradPlot(gradPlot < 0.25)*2 + 0.0005;
% gradPlot(gradPlot < 0.5) = gradPlot(gradPlot < 0.5)*2;
% gradPlot(gradPlot < 1) = gradPlot(gradPlot < 1)*2;
% gradPlot(gradPlot < 1.5) = gradPlot(gradPlot < 1.5)*2;
% gradPlot(gradPlot < 2) = gradPlot(gradPlot < 2)*2;
% gradPlot(gradPlot < 2.5) = gradPlot(gradPlot < 2.5)*2;
% gradPlot(gradPlot < 3) = gradPlot(gradPlot < 3)*2;
% gradPlot(gradPlot < 3.5) = gradPlot(gradPlot < 3.5)*2;
% gradPlot(gradPlot < 4) = gradPlot(gradPlot < 4)*2;
% gradPlot(gradPlot < 4.5) = gradPlot(gradPlot < 4.5)*2;
% gradPlot(gradPlot < 5) = gradPlot(gradPlot < 5)*2;
% gradPlot(gradPlot < 6) = gradPlot(gradPlot < 6)*2;
% gradPlot(gradPlot < 15) = gradPlot(gradPlot < 15)*2;

% Figure for animation
fig1 = figure(1);

% set(fig1, 'Position', [1350 560 560 420]);
% pcolor(gradPlot);
% colormap(flipud(gray));
% % colormap(gray);
% shading interp;

set(fig1, 'Position', [1350 60 560 420]);
contour(partialColorPlot, 10)
colormap(flipud(gray));
% colormap(gray);

pos = get(gca, 'Position');
set(gcf, 'MenuBar', 'None')
set(gca, 'XTick', [], 'YTick', [])
xoffset = 0.05;
pos(1) = pos(1) + xoffset;

drawnow

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

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

while(1)
  %   close all
  
  set(gca, 'Position', pos)
  uicontrol(fig1, 'Style', 'pushbutton', 'String', 'Drop log',...
    'Position', [15 210 60 25],...
    'Callback', @dropLogDown);
  
  bg = uibuttongroup(fig1, 'Position', [0.02 0.60 0.13 0.28], ...
    'Title', 'Log sort',...
    'SelectionChangedFcn',@btnSelection);
  
  % Create three radio buttons in the button group.
  
  uicontrol(bg,'Style','radiobutton',...
    'String','Center',...
    'Position',[10 70 80 30],...
    'HandleVisibility','off',...
    'Tag', 'Center');
  
  uicontrol(bg,'Style', 'radiobutton',...
    'String','Left',...
    'Position',[10 40 80 30],...
    'HandleVisibility','off',...
    'Tag', 'Left');
  
  uicontrol(bg,'Style','radiobutton',...
    'String','Right',...
    'Position',[10 10 80 30],...
    'HandleVisibility','off',...
    'Tag', 'Right');
  
  simulationFile = 'craneLoading.agxLua';
  t = 0;
  timeStep = 1/60.0;
  luaControl = 0;
  
  initInput = luaControl;
  [~, initOutput, ~, ~] = agx('load', simulationFile, initInput, timeStep);
  
  log1_r = initOutput(1);
  log2_r = initOutput(2);
  log3_r = initOutput(3);
  log4_r = initOutput(4);
  
  log1_h = initOutput(5);
  log2_h = initOutput(6);
  log3_h = initOutput(7);
  log4_h = initOutput(8);
  
  crane_r = 0.125;
  crane_length = 6;
  carriage_r = 1.113;
  base_d = 1.07*5;
  gripper_d = 3.5;
  
  carriage = line([4.884 7.927]*7 + 64, [10.167 10.167] + 64, 'LineWidth', carriage_r*2*15, 'Color', [0.2608, 0.2608, 0.8863]);
  log1 = line([-6 -6]*7 + 64, [0, log1_h]*7 + 64, 'LineWidth', log1_r*2*20, 'Color', [0.5608 0.7863 0.5608], 'ButtonDownFcn', @log1Fcn);
  log2 = line([-5 -5]*7 + 64, [0, log2_h]*7 + 64, 'LineWidth', log2_r*2*20, 'Color', [0.5608 0.7863 0.5608], 'ButtonDownFcn', @log2Fcn);
  log3 = line([5 5]*7 + 64, [0, log3_h]*7 + 64, 'LineWidth', log3_r*2*20, 'Color', [0.5608 0.7863 0.5608], 'ButtonDownFcn', @log3Fcn);
  log4 = line([6 6]*7 + 64, [0, log4_h]*7 + 64, 'LineWidth', log4_r*2*20, 'Color', [0.5608 0.7863 0.5608], 'ButtonDownFcn', @log4Fcn);
  crane = line([0, 0] + 64, [0, crane_length]*7 + 64, 'LineWidth', crane_r*2*20, 'Color', [0.2608, 0.2608, 0.8863]);
  craneBase = rectangle('Position', [(-base_d/2 + 64) (-base_d/2 + 64) base_d base_d], 'Curvature', 0.2, ...
    'FaceColor', [0.2608, 0.2608, 0.8863], 'EdgeColor', [0.2608, 0.2608, 0.8863]);
  gripper = rectangle('Position', [(0 - gripper_d/2 + 64) (crane_length*7 - gripper_d/2 + 64) gripper_d gripper_d], 'Curvature', 1, ...
    'FaceColor', [0.2608, 0.2608, 0.8863], 'EdgeColor', [0.2608, 0.2608, 0.8863]);
  
  %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
  % 7 targetColor
  
  %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 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;
  
  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);
    
    %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];
    
    p_x = p(1);
    p_y = p(2);
    
    log1Angle = acos(logList(1, 4:5)*[1 0]') + pi/2;
    log2Angle = acos(logList(2, 4:5)*[1 0]') + pi/2;
    log3Angle = acos(logList(3, 4:5)*[1 0]') + pi/2;
    log4Angle = acos(logList(4, 4:5)*[1 0]') + pi/2;
    
    if (mod(t, 5*timeStep) < timeStep)
      log1.XData = -[(logList(1, 2) - log1_h/2*cos(log1Angle)), logList(1, 2) + log1_h/2*cos(log1Angle)]*7 + 64;
      log1.YData = [logList(1, 1) + log1_h/2*sin(log1Angle), logList(1, 1) - log1_h/2*sin(log1Angle)]*7 + 64;
      log1.Color = [0.4608 0.7863 0.4608];
      log2.XData = -[(logList(2, 2) - log2_h/2*cos(log2Angle)), logList(2, 2) + log2_h/2*cos(log2Angle)]*7 + 64;
      log2.YData = [logList(2, 1) + log2_h/2*sin(log2Angle), logList(2, 1) - log2_h/2*sin(log2Angle)]*7 + 64;
      log2.Color = [0.4608 0.7863 0.4608];
      log3.XData = -[(logList(3, 2) - log3_h/2*cos(log3Angle)), logList(3, 2) + log3_h/2*cos(log3Angle)]*7 + 64;
      log3.YData = [logList(3, 1) + log3_h/2*sin(log3Angle), logList(3, 1) - log3_h/2*sin(log3Angle)]*7 + 64;
      log3.Color = [0.4608 0.7863 0.4608];
      log4.XData = -[(logList(4, 2) - log4_h/2*cos(log4Angle)), logList(4, 2) + log4_h/2*cos(log4Angle)]*7 + 64;
      log4.YData = [logList(4, 1) + log4_h/2*sin(log4Angle), logList(4, 1) - log4_h/2*sin(log4Angle)]*7 + 64;
      log4.Color = [0.4608 0.7863 0.4608];
      
      carriage.Color = [0.2608, 0.2608, 0.8863];
      
      if (logGrabbed)
        carriage.Color = [0 1 0];
      else
        if logRand == 1
          log1.Color = [0 1 0];
        elseif logRand == 2
          log2.Color = [0 1 0];
        elseif logRand == 3
          log3.Color = [0 1 0];
        elseif logRand == 4
          log4.Color = [0 1 0];
        end
      end
      
      crane.XData = [0 -p_y]*7 + 64;
      crane.YData = [0 p_x]*7 + 64;
      gripper.Position = [(-p_y*7 - gripper_d/2 + 64) (p_x*7 - gripper_d/2 + 64) gripper_d gripper_d];
      drawnow
    end
    
    %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) 0.0];
    
    %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
  
  
  disp('Simulation done...')
  t
  simTime
  
  toc
  
  pause(3)
  
  delete(log1)
  delete(log2)
  delete(log3)
  delete(log4)
  delete(crane)
  delete(carriage)
end
end

