


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

    this.lastActiveButton = {
      safetyMode : null, 
      frankaMode : null, 
      operationMode : null, 
    };
  }



    // Operation Mode
   modeStatus(message) {
    var operationModeToButtonMap = {
      "stopped" : "stoppedModeButton", 
      "standby" : "standbyModeButton", 
      "teach" : "teachingModeButton", 
      "execution" : "executionModeButton", 
      "ready" : "readyModeButton"
    }

    if (this.namespace == "left") 
    {
      document.getElementById("recordingStartButton").disabled = true
      document.getElementById("executionStartButton").disabled = true
    }

    if (message.data == "ready")
    {
      if (this.namespace == "left") 
      {
        document.getElementById("recordingStartButton").disabled = false
        document.getElementById("executionStartButton").disabled = false
      }
    }

    
    var operationModeLastActive = this.lastActiveButton.operationMode;

    if(operationModeLastActive && operationModeLastActive != message.data) {
      var operationModeButton = operationModeToButtonMap[operationModeLastActive];
      if (operationModeButton) {
        document.getElementById(this.namespace+"_"+operationModeButton).classList.remove("active");
      }
    }

    var operationModeButton = operationModeToButtonMap[message.data];
    if (operationModeButton) {
      document.getElementById(this.namespace+"_"+operationModeButton).classList.add('active');
    }

    this.lastActiveButton.operationMode = message.data;
    
  }

  // FrankaState
  frankaStatus(message) {
    var frankaModeToButtonMap = {
      0 : "frankaModeOther", 
      1 : "frankaModeIdle", 
      2 : "frankaModeMove", 
      3 : "frankaModeGuiding",
      4 : "frankaModeReflex", 
      5 : "frankaModeUserStopped",
      6 : "frankaModeErrorRecovery" 
    }

    var frankaModeLastActive = this.lastActiveButton.frankaMode;

    if(frankaModeLastActive && frankaModeLastActive != message.robot_mode) {
      var frankaButton = frankaModeToButtonMap[frankaModeLastActive];
      if (frankaButton) {
        document.getElementById(this.namespace+"_"+frankaButton).getElementsByTagName("input")[0].checked = false;
        document.getElementById(this.namespace+"_"+frankaButton).getElementsByTagName("input")[0].disabled = true;
      }
    }
    var frankaButton = frankaModeToButtonMap[message.robot_mode];
    if (frankaButton) {
      document.getElementById(this.namespace+"_"+frankaButton).getElementsByTagName("input")[0].disabled = false;
      document.getElementById(this.namespace+"_"+frankaButton).getElementsByTagName("input")[0].checked = true;
    }

    if(message.robot_mode == 0 || message.robot_mode == 4) {
      document.getElementById(this.namespace+"_"+"fixFrankaButton").classList.add("danger");
    } else {
      document.getElementById(this.namespace+"_"+"fixFrankaButton").classList.remove("danger");
    }

    //if(message.robot_mode == 1) {
    //  document.getElementById(this.namespace+"_"+"fixFrankaButton").classList.add("warning");
    //} else {
    //  document.getElementById(this.namespace+"_"+"fixFrankaButton").classList.remove("warning");
    //}

    this.lastActiveButton.frankaMode = message.robot_mode;
  }


  safetyStatus(message) {
    var safetyStatusToButtonMap = {
      "stopped" : "safetyStopped", 
      "running" : "safetyRunning"
    }

    var safetyButtonLastActive = this.lastActiveButton.safetyMode;

    if(safetyButtonLastActive && safetyButtonLastActive != message.data) {
      var safetyButton = safetyStatusToButtonMap[safetyButtonLastActive];
      if(safetyButton) {
        document.getElementById(this.namespace+"_"+safetyButton).getElementsByTagName("input")[0].checked = false;
        document.getElementById(this.namespace+"_"+safetyButton).getElementsByTagName("input")[0].disabled = true;
      }
    }

    var safetyButton = safetyStatusToButtonMap[message.data];
    if(safetyButton) {
      document.getElementById(this.namespace+"_"+safetyButton).getElementsByTagName("input")[0].disabled = false;
      document.getElementById(this.namespace+"_"+safetyButton).getElementsByTagName("input")[0].checked = true;
    }

    this.lastActiveButton.safetyMode = message.data;
  }
}

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

var lastActiveButton = {
  teachingMode : null, 
  executionMode : null
}

// 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))
  {
    //consider adding warning buttons
  } else {
    //document.getElementById("calibrateButton").disabled = true;
  }
});

// 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';
  } 
  else if (message.data == "running")
  {
    //document.getElementById("RecordingStatusLabel").innerHTML = 'Running!';
  }

  var teachingModeToButtonMap = {
    "stopped" : "recordingStopButton", 
    "running" : "recordingStartButton"
  }

  var teachingModeLastActive = lastActiveButton.teachingMode;

  if(teachingModeLastActive && teachingModeLastActive != message.data) {
    var teachingModeButton = teachingModeToButtonMap[teachingModeLastActive];
    if (teachingModeButton) {
      document.getElementById(teachingModeButton).classList.remove("active");
    }
  }

  var teachingModeButton = teachingModeToButtonMap[message.data];
  if (teachingModeButton) {
    document.getElementById(teachingModeButton).classList.add("active");
  }

  lastActiveButton.teachingMode = message.data;
});

// 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';
  } 
  else if (message.data == "running")
  {
    //document.getElementById("ExecutionStatusLabel").innerHTML = 'RUNNING!';
  }
  var executionModeToButtonMap = {
    "stopped" : "executionStopButton", 
    "running" : "executionStartButton"
  }

  var executionModeLastActive = lastActiveButton.executionMode;

  if(executionModeLastActive && executionModeLastActive != message.data) {
    var executionModeButton = executionModeToButtonMap[executionModeLastActive];
    if (executionModeButton) {
      document.getElementById(executionModeButton).classList.remove("active");
    }
  }

  var executionModeButton = executionModeToButtonMap[message.data];
  if (executionModeButton) {
    document.getElementById(executionModeButton).classList.add("active");
  }

  lastActiveButton.executionMode = message.data;
});


// 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, toSend)
{

  if(!toSend) {
    serviceType = 'std_srvs/demobot.Trigger'
  } else {
    serviceType = 'std_srvs/demobot.TriggerMessage'
  }

  var service = new ROSLIB.Service({
    ros : ros,
    name : serviceName,
    serviceType : serviceType
  });

  if(!toSend) {
    var request = new ROSLIB.ServiceRequest({});
  } else {
    var toSend = JSON.stringify(toSend);
    var request = new ROSLIB.ServiceRequest({message : toSend});  
  }


  /*
  var service = new ROSLIB.Service({
    ros : ros,
    name : serviceName,
    serviceType : 'std_srvs/demobot.TriggerMessage'
  });

  if(toSend) {
    toSend = JSON.stringify(toSend)
  } else {
    toSend = ""
  }
  */
  

  var request = new ROSLIB.ServiceRequest({message : toSend});  


  var serviceCallTimeStamp = Date.now();
  var serviceCallReturned = false;
  service.callService(request, function(result) {
    console.log('Result for service call on '
      + serviceName
      + ': '
      + result);
      //check for result.success == false
      if(result.success == true) {
        $(document).ready(function(){
          $.notify({
            message: "Going good! "+ serviceName
          }, {
            "placement" : {
              from: "bottom",
              align: "right"
            }, 
            type: 'success', 
            delay: 1000 
          });
        });
      } else {
        $(document).ready(function(){
          $.notify({
            message: "Something went wrong :( " + serviceName
          }, {
            "placement" : {
              from: "bottom",
              align: "right"
            }, 
            type: 'danger', 
            delay: 1000 
          });
        });
      }
      serviceCallReturned = true;
  }, function(error){
      console.log("errored");
      $(document).ready(function(){
        $.notify({
          message: "Something went wrong :( " + serviceName
        }, {
          "placement" : {
            from: "bottom",
            align: "right"
          }, 
          type: 'danger', 
          delay: 1000 
        });
      });
      serviceCallReturned = true;
  });
  var responseCheckInterval = setInterval(function() {
    var timeElapsedSinceServiceCall = Date.now() - serviceCallTimeStamp; // milliseconds elapsed since start
    if(serviceCallReturned) {
      clearInterval(responseCheckInterval);
      return;
    } else if (timeElapsedSinceServiceCall > 3000){
        $(document).ready(function(){
          $.notify({
            message: "Timeout on " + serviceName
          }, {
            "placement" : {
              from: "bottom",
              align: "right"
            }, 
            type: 'danger', 
            delay: 1000 
          });
        });
        clearInterval(responseCheckInterval);
        return;
    }
  }, 1000);
}

function loadSelectedBagFile() {
  var selectedBagName = $('#bagList').val();

  var toSend = {
    message : selectedBagName
  }

  triggerService('/aescape/bags/startPlayingRecording', toSend)
}



document.addEventListener("keydown", invokeSafetyController);
function invokeSafetyController(event)
{
   var keyID = event.keyCode;
   //8 == backspace, 46 == delete
   if(keyID == 46) {
    triggerService('/aescape/safety_activate')
   }
}





