///////////////////////////////////////////////////////////////////////////////////
// Publishers
///////////////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////
// Topics
////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////
// Subscribers
////////////////////////////////////////////////////////////////

// Operation Mode
var modeStatus = new ROSLIB.Topic({
  ros : ros,
  name : '/aescape/mode/status',
  messageType : 'std_msgs/String',
  throttle_rate : 500 // 2Hz
});

modeStatus.subscribe(function(message) {
  document.getElementById("stoppedModeButton").className = "btn btn-secondary"
  document.getElementById("standbyModeButton").className = "btn btn-primary"
  document.getElementById("teachingModeButton").className = "btn btn-primary"
  document.getElementById("executionModeButton").className = "btn btn-primary"


  if (message.data == "stopped") {
    document.getElementById("stoppedModeButton").className = "btn btn-warning"
  } else if (message.data == "standby")
  {
    document.getElementById("standbyModeButton").className = "btn btn-primary btn-success"
  } else if (message.data == "teach")
  {
    document.getElementById("teachingModeButton").className = "btn btn-primary btn-success"
  } else if (message.data == "execution")
  {
    document.getElementById("executionModeButton").className = "btn btn-primary btn-success"
  }
});

// Playing Bag
var bagPlayingTopic = new ROSLIB.Topic({
  ros : ros,
  name : '/aescape/bags/playing',
  messageType : 'std_msgs/String'
});

bagPlayingTopic.subscribe(function(message) {
  document.getElementById("bagPlayingText").innerHTML = message.data
});

// FrankaState
var frankaStatus = new ROSLIB.Topic({
  ros : ros,
  name : '/franka_state_controller/franka_states',
  messageType : 'franka_msgs/FrankaState',
  throttle_rate : 500 // 2Hz
});

frankaStatus.subscribe(function(message) {
  document.getElementById("frankaModeOther").className = "label label-default"
  document.getElementById("frankaModeIdle").className = "label label-default"
  document.getElementById("frankaModeMove").className = "label label-default"
  document.getElementById("frankaModeGuiding").className = "label label-default"
  document.getElementById("frankaModeReflex").className = "label label-default"
  document.getElementById("frankaModeUserStopped").className = "label label-default"
  document.getElementById("frankaModeErrorRecovery").className = "label label-default"
  document.getElementById("fixFrankaButton").className = "btn btn-primary"

  if (message.robot_mode == 0)
  { 
    document.getElementById("frankaModeOther").className = "label label-warning"
    document.getElementById("fixFrankaButton").className = "btn btn-danger"
  } 
  else if (message.robot_mode == 1)
  {
    document.getElementById("frankaModeIdle").className = "label label-warning"
    document.getElementById("fixFrankaButton").className = "btn btn-danger"
  } 
  else if (message.robot_mode == 2)
  {
    document.getElementById("frankaModeMove").className = "label label-warning"
    document.getElementById("fixFrankaButton").className = "btn btn-success"
  } 
  else if (message.robot_mode == 3) 
  {
    document.getElementById("frankaModeGuiding").className = "label label-warning"
    document.getElementById("fixFrankaButton").className = "btn btn-default"
  }
  else if (message.robot_mode == 4)
  {
    document.getElementById("frankaModeReflex").className = "label label-warning"
    document.getElementById("fixFrankaButton").className = "btn btn-danger"

  }
  else if (message.robot_mode == 5) 
  {
    document.getElementById("frankaModeUserStopped").className = "label label-warning"
    document.getElementById("fixFrankaButton").className = "btn btn-default"
  }
  else if (message.robot_mode == 6)
  {
    document.getElementById("frankaModeErrorRecovery").className = "label label-warning"
    document.getElementById("fixFrankaButton").className = "btn btn-default"
  }
});


// Robotiq Data
var robotiqDataTopic = new ROSLIB.Topic({
  ros : ros,
  name : '/robotiq_ft_wrench',
  messageType : 'geometry_msgs/WrenchStamped',
  throttle_rate : 500 // 2Hz
});

robotiqDataTopic.subscribe(function(message) {
  var force = message.wrench.force
  document.getElementById("robotiqX").innerHTML = force.x.toFixed(1)
  document.getElementById("robotiqY").innerHTML = force.y.toFixed(1)
  document.getElementById("robotiqZ").innerHTML = force.z.toFixed(1)

  if ((Math.abs(force.x) > 0.5) || (Math.abs(force.y) > 0.5) || (Math.abs(force.z) > 0.5))
  {
    document.getElementById("calibrateButton").className = "btn btn-danger"
  } else {
    document.getElementById("calibrateButton").className = "btn btn-default"
  }
});

////////////////////////////////////////////////////////////////
// Services
////////////////////////////////////////////////////////////////


function triggerService(serviceName)
{
  var service = new ROSLIB.Service({
    ros : ros,
    name : serviceName,
    serviceType : 'std_srvs/Trigger'
  });
  
  var request = new ROSLIB.ServiceRequest({});
  
  service.callService(request, function(result) {
    console.log('Result for service call on '
      + serviceName
      + ': '
      + result.sum);
  });
}


function triggerMessageService(serviceName, textInput)
{
  var text = document.getElementById(textInput).value

  var service = new ROSLIB.Service({
    ros : ros,
    name : serviceName,
    serviceType : 'demobot.TriggerMessage'
  });
  
  var request = new ROSLIB.ServiceRequest({
    message : text
  });
  
  service.callService(request, function(result) {
    console.log('Result for service call on '
      + serviceName
      + ': '
      + result.sum);
  });
}