


///////////////////////////////////////////////////////////////////////////////////
// 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.frankaForceObservedTopic = new ROSLIB.Topic({
      ros : ros,
      name : '/'+this.namespace+'/monitor/robot_goal', 
      messageType : 'aescape_control/RobotGoal',
      throttle_rate : 500 // 2Hz
    });

    this.frankaForceObservedTopic.subscribe(this.frankaForce.bind(this, 'observed'));

    this.frankaForceCommandedTopic = new ROSLIB.Topic({
      ros : ros,
      name : '/'+this.namespace+'/robot_goal', 
      messageType : 'aescape_control/RobotGoal',
      throttle_rate : 500 // 2Hz
    });

    this.frankaForceCommandedTopic.subscribe(this.frankaForce.bind(this, 'commanded'));
    
    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;
  }

  frankaForce(forceTypeCode, message) {
    if(this.namespace == 'left') {
      var bindedArm = "Left";
    }
    if(this.namespace == 'right') {
      var bindedArm = "Right";
    }
    if (forceTypeCode == 'observed') {
      var forceType = "Observed";
    }
    if (forceTypeCode == 'commanded') {
      var forceType = "Commanded";
    }

    if(this.namespace == 'right' && forceTypeCode == 'commanded') {
      console.log("frankaForce");
      console.log(message)
    }

    var force = message.wrench.force
    document.getElementById("frankaForce" + bindedArm + forceType + "X").innerHTML = force.x.toFixed(1)
    document.getElementById("frankaForce" + bindedArm + forceType + "Y").innerHTML = force.y.toFixed(1)
    document.getElementById("frankaForce" + bindedArm + forceType + "Z").innerHTML = force.z.toFixed(1)
  
    var totalForce = math.norm([force.x, force.y, force.z]).toFixed(1)
    var totalForcePercent = totalForce/2
  
    var doc = document.getElementById("frankaForce" + forceType + bindedArm);
    doc.style.width = totalForcePercent + "%";
    doc.innerHTML = totalForce + " N";
    doc.setAttribute('aria-valuenow', totalForce);
  }

}

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/monitor/eef_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;

  //status changed
  if(teachingModeLastActive && teachingModeLastActive != message.data) {
    var teachingModeButton = teachingModeToButtonMap[teachingModeLastActive];
    if (teachingModeButton) {
      document.getElementById(teachingModeButton).classList.remove("active");
    }
    if(message.data == 'stopped') {
      triggerService('/left/aescape/mode/activateReadyController');
      triggerService('/right/aescape/mode/activateReadyController');
      displayTaggingOptions('teach');
    }
  }

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

  lastActiveButton.teachingMode = message.data;
});


// Execution bag
// Use playback status instead of this
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;
});


// Playback status
var playbackStatus = new ROSLIB.Topic({
  ros : ros,
  name : '/left/playback/status',
  messageType : 'std_msgs/String'
});

playbackStatus.subscribe(function(message) {
  console.log("playbackStatus");
  console.log(message);
  message = JSON.parse(message.data);
  status = message.status;
  bagName = message.bag_file;

  if (status === "playing") {

  } else if (message === "stopped")
  {
    triggerService('/left/aescape/mode/activateReadyController');
    triggerService('/right/aescape/mode/activateReadyController');
    displayTaggingOptions('play');
  }
});


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

lastPlayedBag.subscribe(function(message) {
  //document.getElementById("lastbagText").innerHTML = message.data
  console.log("Last played bag");
  console.log(message);
});


//Last recorded bag
var lastRecordedBag = new ROSLIB.Topic({
  ros : ros,
  name : '/aescape/bags/last_recorded',
  messageType : 'std_msgs/String'
});

lastRecordedBag.subscribe(function(message) {
  //document.getElementById("lastbagText").innerHTML = message.data
  console.log("Last recorded bag");
  console.log(message);
});


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

function triggerService(serviceName, toSend, callback) {

  disableUI();

  var callback = callback || $.noop;

  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 hellobiatch = "hellobiatch"


  /*
  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) {
      serviceCallReturned = true;
      if(result.success == true) {
        $(document).ready(function(){
          $.notify({
            message: "Going good! "+ serviceName
          }, {
            "placement" : {
              from: "bottom",
              align: "right"
            }, 
            type: 'success', 
            delay: 1000 
          });
        });
        enableUI();
        return callback(null, {success : true});
      } else {
        $(document).ready(function(){
          $.notify({
            message: "Something went wrong :( " + serviceName
          }, {
            "placement" : {
              from: "bottom",
              align: "right"
            }, 
            type: 'danger', 
            delay: 1000 
          });
        });
        enableUI();
        return callback("Something went wrong :( " + serviceName, {success : false});
      }
  }, function(error){
      $(document).ready(function(){
        serviceCallReturned = true;
        $.notify({
          message: "Something went wrong :( " + serviceName
        }, {
          "placement" : {
            from: "bottom",
            align: "right"
          }, 
          type: 'danger', 
          delay: 1000 
        });
      });
      enableUI();
      return callback("Something went wrong :( " + serviceName, {success : false})
  });
  var responseCheckInterval = setInterval(function() {
    var timeElapsedSinceServiceCall = Date.now() - serviceCallTimeStamp; // milliseconds elapsed since start
    if(serviceCallReturned) {
      clearInterval(responseCheckInterval);
      return;
    } else if (timeElapsedSinceServiceCall > 7000){
        $(document).ready(function(){
          $.notify({
            message: "Timeout on " + serviceName
          }, {
            "placement" : {
              from: "bottom",
              align: "right"
            }, 
            type: 'danger', 
            delay: 1000 
          });
        });
        clearInterval(responseCheckInterval);
        enableUI();
        return;
    }
  }, 1000);
}


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

function setEEFConfig(arm, config) {
  var operationModeSelector = '#' + arm + '-operation-mode';
  document.querySelectorAll(operationModeSelector + ' button').forEach(function(element){
    element.classList.add('disabled')
  })
  var serviceMap = {
    teach : 'setTrainingEEF', 
    play : 'setExecutionEEF'
  }
  var service = '/' + arm + '/aescape/hardware/' + serviceMap[config]
  triggerService(service, false, function(error, result){
    document.querySelectorAll(operationModeSelector + ' button').forEach(function(element){
      element.classList.remove('disabled')
    })
  })
}

function startTeachRecoding() {
  async.waterfall([
      function(callback) {
          triggerService('/left/aescape/hardware/calibrateRobotiq', false, function(err, res){
            callback(err, res);
          });
      },
      function(response, callback) {
          triggerService('/left/aescape/mode/activateTeachingController', false, callback);
      },
      function(response, callback) {
          triggerService('/aescape/bags/startTeachRecording', false, callback)
      }
  ], function (err, result) {
    console.log("starting teach recoding");
  });
}

function stopTeachRecording() {
  async.waterfall([
      function(callback) {
          triggerService('/aescape/bags/stopTeachRecording', false, callback);
      },
      // function(response, callback) {
      //     triggerService('/left/aescape/mode/activateReadyController', false, callback);
      // },
      function(response, callback) {
        //displayTaggingOptions('teach');
        callback();
      }
  ], function (err, result) {
    console.log(err);
  });
}


function playLoadedRecording() {
  async.waterfall([
      function(callback) {
        triggerService('/left/aescape/mode/activateExecutionController', false, callback); 
      },
      function(response, callback) {
        triggerTopic('/left/run_trajectory', false, callback)
      }
  ], function (err, result) {

  });
}

function stopPlayingRecording() {
  async.waterfall([
      function(callback) {
          triggerService('/aescape/bags/stopPlayingBag', false, callback);
      },
      // function(response, callback) {
      //     triggerService('/left/aescape/mode/activateReadyController', false, callback);
      // },
      function(response, callback) {
        //displayTaggingOptions('play');
        callback();
      }
  ], function (err, result) {
      // result now equals 'done'
  });
}





