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

function triggerTopic(topicName)
{
  var topic = new ROSLIB.Topic({
    ros : ros,
    name : topicName,
    messageType : 'std_msgs/Empty'
  });
  
  var msg = new ROSLIB.Message({});

  topic.publish(msg);
}

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



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

class ArmControls 
{
  constructor(namespace) {
    this.namespace = namespace;

    // Franka Status
    this.frankaStatusTopic = new ROSLIB.Topic({
      ros : ros,
      name : '/'+this.namespace+'/franka_state_controller/franka_states',
      messageType : 'franka_msgs/FrankaState',
      throttle_rate : 500 // 2Hz
    });
    this.frankaStatusTopic.subscribe(this.frankaStatus.bind(this));

    // Mode Status
    this.modeStatusTopic = new ROSLIB.Topic({
      ros : ros,
      name : '/'+this.namespace+'/aescape/mode/status',
      messageType : 'std_msgs/String',
      throttle_rate : 500 // 2Hz
    });
    this.modeStatusTopic.subscribe(this.modeStatus.bind(this));

    // Safety Status
    this.safetyStatusTopic = new ROSLIB.Topic({
      ros : ros,
      name : '/'+this.namespace+'/aescape/mode/safety_status',
      messageType : 'std_msgs/String',
      throttle_rate : 500 // 2Hz
    });
    this.safetyStatusTopic.subscribe(this.safetyStatus.bind(this));

  }

  // FrankaState
  frankaStatus(message) {
    document.getElementById(this.namespace+"_frankaModeOther").className = "label label-default"
    document.getElementById(this.namespace+"_frankaModeIdle").className = "label label-default"
    document.getElementById(this.namespace+"_frankaModeMove").className = "label label-default"
    document.getElementById(this.namespace+"_frankaModeGuiding").className = "label label-default"
    document.getElementById(this.namespace+"_frankaModeReflex").className = "label label-default"
    document.getElementById(this.namespace+"_frankaModeUserStopped").className = "label label-default"
    document.getElementById(this.namespace+"_frankaModeErrorRecovery").className = "label label-default"
    document.getElementById(this.namespace+"_fixFrankaButton").className = "btn btn-primary"

    if (message.robot_mode == 0)
    { 
      document.getElementById(this.namespace+"_frankaModeOther").className = "label label-warning"
      document.getElementById(this.namespace+"_fixFrankaButton").className = "btn btn-danger"
    } 
    else if (message.robot_mode == 1)
    {
      document.getElementById(this.namespace+"_frankaModeIdle").className = "label label-warning"
      document.getElementById(this.namespace+"_fixFrankaButton").className = "btn btn-danger"
    } 
    else if (message.robot_mode == 2)
    {
      document.getElementById(this.namespace+"_frankaModeMove").className = "label label-warning"
      document.getElementById(this.namespace+"_fixFrankaButton").className = "btn btn-success"
    } 
    else if (message.robot_mode == 3) 
    {
      document.getElementById(this.namespace+"_frankaModeGuiding").className = "label label-warning"
      document.getElementById(this.namespace+"_fixFrankaButton").className = "btn btn-default"
    }
    else if (message.robot_mode == 4)
    {
      document.getElementById(this.namespace+"_frankaModeReflex").className = "label label-warning"
      document.getElementById(this.namespace+"_fixFrankaButton").className = "btn btn-danger"

    }
    else if (message.robot_mode == 5) 
    {
      document.getElementById(this.namespace+"_frankaModeUserStopped").className = "label label-warning"
      document.getElementById(this.namespace+"_fixFrankaButton").className = "btn btn-default"
    }
    else if (message.robot_mode == 6)
    {
      document.getElementById(this.namespace+"_frankaModeErrorRecovery").className = "label label-warning"
      document.getElementById(this.namespace+"_fixFrankaButton").className = "btn btn-default"
    }
  }

    // Operation Mode
   modeStatus(message) {
    document.getElementById(this.namespace+"_stoppedModeButton").className = "btn btn-secondary"
    document.getElementById(this.namespace+"_standbyModeButton").className = "btn btn-primary"
    document.getElementById(this.namespace+"_readyModeButton").className = "btn btn-primary"
    document.getElementById(this.namespace+"_teachingModeButton").className = "btn btn-primary"
    document.getElementById(this.namespace+"_executionModeButton").className = "btn btn-primary"
    
    if (this.namespace == "left") 
    {
      document.getElementById("recordingStartButton").disabled = true
      document.getElementById("executionStartButton").disabled = true
    }
   
    if (message.data == "stopped") 
    {
      document.getElementById(this.namespace+"_stoppedModeButton").className = "btn btn-warning"
    } 
    else if (message.data == "standby")
    {
      document.getElementById(this.namespace+"_standbyModeButton").className = "btn btn-primary btn-success"
    } 
    else if (message.data == "teach")
    {
      document.getElementById(this.namespace+"_teachingModeButton").className = "btn btn-primary btn-success"
    } 
    else if (message.data == "execution")
    {
      document.getElementById(this.namespace+"_executionModeButton").className = "btn btn-primary btn-success"
    }
    else if (message.data == "ready")
    {
      document.getElementById(this.namespace+"_readyModeButton").className = "btn btn-primary btn-success"
      
      if (this.namespace == "left") 
      {
        document.getElementById("recordingStartButton").disabled = false
        document.getElementById("executionStartButton").disabled = false
      }
    }
  }

  safetyStatus(message) {
    document.getElementById(this.namespace+"_safetyRunning").className = 'label label-default';
    document.getElementById(this.namespace+"_safetyStopped").className = 'label label-default';
    
    if (message.data == "stopped") 
    {
      document.getElementById(this.namespace+"_safetyStopped").className = 'label label-danger';
    } 
    else if (message.data == "running")
    {
      document.getElementById(this.namespace+"_safetyRunning").className = 'label label-success';
    }
  };
}

var leftArmControls = new ArmControls('left');
var rightArmControls = new ArmControls('right');
  

// Robotiq Data
var robotiqDataTopic = new ROSLIB.Topic({
  ros : ros,
  name : '/left/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)

  var totalForce = math.norm([force.x, force.y, force.z]).toFixed(1)
  var totalForcePercent = totalForce/2

  doc = document.getElementById("robotiqForceBar");
  doc.style.width = totalForcePercent + "%";
  doc.innerHTML = totalForce + " N";
  doc.setAttribute('aria-valuenow', totalForce);

  
  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"
  }
});


// Recording Bag
var recordingBagTopic = new ROSLIB.Topic({
  ros : ros,
  name : '/aescape/bags/recording_status',
  messageType : 'std_msgs/String'
});

recordingBagTopic.subscribe(function(message) {
  if (message.data == "stopped") 
  {
    document.getElementById("RecordingStatusLabel").innerHTML = 'Not Running';
    document.getElementById("RecordingStatusLabel").className = 'label label-warning';
  } 
  else if (message.data == "running")
  {
    document.getElementById("RecordingStatusLabel").innerHTML = 'RUNNING!';
    document.getElementById("RecordingStatusLabel").className = 'label label-success';
  }
});

// Executing Bag
var executingBagTopic = new ROSLIB.Topic({
  ros : ros,
  name : '/aescape/bags/execution_status',
  messageType : 'std_msgs/String'
});

executingBagTopic.subscribe(function(message) {
  if (message.data == "stopped") 
  {
    document.getElementById("ExecutionStatusLabel").innerHTML = 'Not Running';
    document.getElementById("ExecutionStatusLabel").className = 'label label-warning';
  } 
  else if (message.data == "running")
  {
    document.getElementById("ExecutionStatusLabel").innerHTML = 'RUNNING!';
    document.getElementById("ExecutionStatusLabel").className = 'label label-success';
  }
});


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

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

////////////////////////////////////////////////////////////////
// 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);
  });
}


// function executeBagForm()
function updateSelectedBagFile(newFile)
{
  // var newFile = document.getElementById(newFileInput).innerHTML
  document.getElementById("bagNameText").value = newFile
}