%
rosshutdown;
clear;

rosinit;
% MasterHost will be your lab PC IP:
masterHost = 'HERE';
matlab_node = robotics.ros.Node('your_node', masterHost);

% Publishers
jointSpeedPub = robotics.ros.Publisher(matlab_node,'/joint_group_vel_controller/command','std_msgs/Float64MultiArray');

% Subscribers
jointStateSub = robotics.ros.Subscriber(matlab_node,'/joint_states');
twist1scaleSub = robotics.ros.Subscriber(matlab_node,'/twist_1_scale');
twist2scaleSub = robotics.ros.Subscriber(matlab_node,'/twist_2_scale');
twist3scaleSub = robotics.ros.Subscriber(matlab_node,'/twist_3_scale');
workModeSub = robotics.ros.Subscriber(matlab_node,'/work_mode');

% Create the SerialLink robot (UR3 model in Robotics toolbox):
% HERE
% Locate the tool frame at 15 cm in the z-directon:
% HERE

% Create the twists T1, T2, T3 in the adequate frame according to the motion
% required in each working mode.
% Workmode A:
% HERE
% Workmode B:
% HERE


% loop at (10hz)
r = robotics.Rate(50);
while robotics.ros.internal.Global.isNodeActive

  % update position
  if isvalid(jointStateSub)
    jointStateMsg = receive(jointStateSub);
    q(1) = jointStateMsg.Position(3);
    q(2) = jointStateMsg.Position(2);
    q(3) = jointStateMsg.Position(1);
    q(4) = jointStateMsg.Position(4);
    q(5) = jointStateMsg.Position(5);
    q(6) = jointStateMsg.Position(6);
  end
  %display(q)

  % Update jacobian
  % HERE

  % read work mode work_mode
  if isvalid(workModeSub)
    workModeMsg = receive(workModeSub);
    work_mode = workModeMsg.Data;
    % display(work_mode)
  end

  % read scales s1, s2, s3
  if isvalid(twist1scaleSub)
    slider1Msg = receive(twist1scaleSub);
    s1 = slider1Msg.Data;
  end
  if isvalid(twist2scaleSub)
    slider2Msg = receive(twist2scaleSub);
    s2 = slider2Msg.Data;
  end
  if isvalid(twist3scaleSub)
    slider3Msg = receive(twist3scaleSub);
    s3 = slider3Msg.Data;
  end

  % Desired differential motion in the new frame
  % [dx, dy, dz, rx, ry, rz]:
  % HERE



  % Associate yout twist with the joystick buttons (s1, s2, s3):
  % HERE




  % display(s1)
  % display(s2)
  % display(s3)



  % Compute joint velocities:
  % HERE

  % Check not to sent q_dot with module greater than 1:
  % HERE


  % publish joint speeds
  jointSpeedMsg = rosmessage('std_msgs/Float64MultiArray');
  jointSpeedMsg.Data(1) = q_dot(1);
  jointSpeedMsg.Data(2) = q_dot(2);
  jointSpeedMsg.Data(3) = q_dot(3);
  jointSpeedMsg.Data(4) = q_dot(4);
  jointSpeedMsg.Data(5) = q_dot(5);
  jointSpeedMsg.Data(6) = q_dot(6);
  send(jointSpeedPub, jointSpeedMsg);
  waitfor(r);
  display(q_dot')
end