webmap.modules.robot = {} /// Namespace for robot modules.


/// Create an SVG element to represent a robot from an image.
/**
 * \param iri         The IRI of the image.
 * \param width       The width of the robot.
 * \param height      The height of the robot.
 * \param origin_left The distance from the left edge of the robot to it's origin.
 * \param origin_top  The distance from the top edge of the robot to it's origin.
 * 
 * Note that the image used must have it's origin at the top left corner. This is always true for
 * raster images, but it is your own responsibility with SVG images. SVG images should always
 * have a viewBox attribute on the root SVG element, or scaling will not work properly.
 * The viewBox attribute should also ensure that the origin lies at the top left corner of the
 * SVG image.
 */
webmap.createImageModel = function(iri, width, height, origin_left, origin_top) {
	if (origin_left == undefined) origin_left = width  * 0.5;
	if (origin_top  == undefined) origin_top  = height * 0.5;
	
	/// The image element.
	var image = document.createElementNS(webmap.svgns, "image");
	image.setAttributeNS(webmap.xlinkns, "href", iri);
	image.setAttribute("x",      0);
	image.setAttribute("y",      0);
	image.setAttribute("width",  width);
	image.setAttribute("height", height);
	image.style.overflow = "visible";
	
	var transform  = webmap.svg.createSVGMatrix()
	// Translate the image so that the origin is at the specifified location.
	.translate(-origin_left, -origin_top)
	// Flip the Y axis of the image, since we're placing it in a Cartesian coordinate grid.
	.translate(width * 0.5, height * 0.5)
	.scaleNonUniform(1, -1)
	.translate(-width * 0.5, -height * 0.5);
	
	image.transform.baseVal.initialize(image.transform.baseVal.createSVGTransformFromMatrix(transform));
	return image;
}



/// Create a new robot.
/**
 * \param name        The name of the robot.
 * \param model       The SVG element to render.
 */
webmap.Robot = function(name, model) {
	
	// Call parent constructor.
	webmap.Extendable.call(this, webmap.modules.robot);
	
	/// Locally unique ID.
	this.id = webmap.generateId();
	
	/// The name of the robot.
	this.name = name;
	
	/// The map the robot is currently associated with.
	this.map = null;
	
	/// True if the robot is selected.
	this.selected = false;
	
	/// Position in 3D of the robot.
	this.position = {x: 0, y: 0, z: 0};
	
	/// Orientation of the robot as quaternion.
	this.orientation = {x: 0, y: 0, z: 0, w: 0};
	
	/// The orientation flattened to the rotation of the robot around the Z axis.
	this.angle = 0;
	
	/// The odometry topic.
	this.odometry_topic = null;
	
	/// The twist topic.
	this.twist_topic = null
	
	/// The move topic.
	this.move_topic = null
	
	/// The name of the base link.
	this.base_link = "base_link";
	
	/// Indicates if the cached odometry information is valid.
	/**
	 * The cached odometry information is valid when the first odometry
	 * message has arrived.
	 */
	this.valid = false;
	
	/// SVG model content representing the robot.
	this.svg = document.createElementNS(webmap.svgns, "g");
	
	// Add the model to the SVG rendering.
	this.svg.appendChild(model.cloneNode(true));
	
	// Add a reference back to the robot from the SVG element.
	this.svg.robot = this;
	
	// Add event listeners.
	this.mouseClickListener = this.mouseClick.bind(this);
	this.svg.addEventListener("click", this.mouseClickListener, false);
}

webmap.extend(webmap.Extendable, webmap.Robot);

/// Clean up the robot.
/**
 * All subscriptions and advertisements are removed, and the robot is removed from it's map.
 */
webmap.Robot.prototype.destroy = function() {
	webmap.Robot.prototype.unsubscribeOdometryTopic();
	webmap.Robot.prototype.unadvertiseTwistTopic();
	webmap.Robot.prototype.unadvertiseMoveTopic();
	if (this.map) this.map.removeRobot(this);
}


/// Set the odometry topic used by the robot.
/**
 * \param connection  The ROS connection for this topic.
 * \param topic       The name of the topic.
 * \param throttle    (Optional) The minimum time in milliseconds between receiving two updates. Defaults to 100.
 * 
 * The topic has to be a Pose, PoseStamped, PoseWithCovarianceStamped
 * or Pose2D message from the geometry_msgs package.
 */
webmap.Robot.prototype.setOdometryTopic = function(connection, topic, throttle) {
	if (throttle === undefined || throttle === null) throttle = 100;
	this.unsubscribeOdometryTopic();
	connection.subscribe(this.handleOdometry.bind(this), topic, null, throttle, null, null, null, this.id);
	this.odometry_topic = {connection: connection, name: topic};
}

/// Unsubscribe the robot from the odometry topic.
/**
 * This method doesn nothing if the robot wasn't subscribed to an odometry topic.
 */
webmap.Robot.prototype.unsubscribeOdometryTopic = function() {
	if (this.odometry_topic) {
		this.odometry_topic.connection.unsubscribe(this.odometry_topic.name, this.id);
		this.odometry_topic = null;
	}
}

/// Set the twist topic used to control the robot.
/**
 * \param connection  The ROS connection for this topic.
 * \param topic       The name of the topic.
 */
webmap.Robot.prototype.setTwistTopic = function(connection, topic) {
	connection.advertise(topic, "geometry_msgs/Twist", this.id);
	this.twist_topic = {connection: connection, name: topic};
}

/// Unadvertise the twist topic
/**
 * This method does nothing if the robot hasn't advertised a twist topic yet.
 */
webmap.Robot.prototype.unadvertiseTwistTopic = function() {
	if (this.twist_topic) {
		this.twist_topic.connection.unadvertise(this.twist_topic.name, this.id);
		this.twist_topic = null;
	}
}

/// Set the move topic used to control the robot.
/**
 * The move topic is used to send 2D position goals for the robot.
 * 
 * \param connection  The ROS connection for this robot.
 * \param topic       The name of the move topic.
 */
webmap.Robot.prototype.setMoveTopic = function(connection, topic) {
	connection.advertise(topic, "move_base_simple/goal", this.id);
	this.move_topic = {connection: connection, name: topic};
}

/// Unadvertise the move topic
/**
 * If there was no move topic yet, nothing happens.
 */
webmap.Robot.prototype.unadvertiseMoveTopic = function() {
	if (this.move_topic) {
		this.move_topic.connection.unadvertise(this.move_topic.name, this.id);
		this.move_topic = null;
	}
}

/// Send a twist message to the robot.
/**
 * \param x  The linear X component.
 * \param y  The linear Y component.
 * \param z  The linear Z component.
 * \param rx The angular X component.
 * \param ry The angular Y component.
 * \param rz The angular Z component.
 */
webmap.Robot.prototype.twist = function(x, y, z, rx, ry, rz) {
	if (this.twist_topic) {
		var msg = {
			linear:  {x:  x, y:  y, z:  z},
			angular: {x: rx, y: ry, z: rz}
		};
		this.twist_topic.connection.publish(this.twist_topic.name, msg);
		this.notifyModules("onSendTwist", msg);
	}
}

/// Send a position goal to the robot.
/**
 * \param position    A 3D vector object with x, y and z attributes representing the desired position of the base link.
 * \param orientation A 4D quaternion object with w, x, y and z attribute representing the desired orientation of the base link.
 */
webmap.Robot.prototype.move = function(position, orientation) {
	if (this.move_topic) {
		msg = {
			frame_id: this.base_link,
			positions: {
				x: position.x,
				y: position.y,
				z: position.z
			},
			orientation: {
				x: orientation.x,
				y: orientation.y,
				z: orientation.z,
				w: orientation.w
			}
		};
		this.move_topic.connection.publish(this.move_topic.name, msg);
		this.notifyModules("onSendMove", msg);
	}
}

/// Set the selected state of the robot.
/**
 * Adds or removes the "selected" class to/from the SVG content.
 * \param selected True to make the robot selected, false to make the robot deselected.
 */
webmap.Robot.prototype.setSelected = function(selected) {
	this.selected = selected;
	if (selected) {
		this.svg.classList.add("selected");
	} else {
		this.svg.classList.remove("selected");
	}
	this.notifyModules("onSelect", selected);
}

/// Process an odometry message.
/**
 * \param msg The odometry message.
 */
webmap.Robot.prototype.handleOdometry = function(msg) {
	var position    = null;
	var orientation = null;
	var angle       = null;
	
	// It's a Pose message.
	if (msg.position && msg.orientation) {
		position    = msg.position;
		orientation = msg.orientation;
		angle       = webmap.quaternionZ(orientation);
	// It's a PoseStamped message.
	} else if (msg.pose && msg.pose.position && msg.pose.orientation) {
		position    = msg.pose.position;
		orientation = msg.pose.orientation;
		angle       = webmap.quaternionZ(orientation);
	// It's a PoseWithCovarianceStamped message.
	} else if (msg.pose && msg.pose.pose && msg.pose.pose.position && msg.pose.pose.orientation) {
		position    = msg.pose.pose.position;
		orientation = msg.pose.pose.orientation;
		angle       = webmap.quaternionZ(orientation);
	// It's a Pose2D message.
	} else if (msg.x && msg.y && msg.theta) {
		position    = {x: msg.x, y: msg.y, z: 0};
		orientation = null;
		angle       = msg.theta;
	}
	
	// Check if anything actually changed.
	// If the old data was invalid, any data is a change.
	var dirty = !this.valid;
	dirty |= !webmap.vectorEqual(this.position, position);
	dirty |= this.angle !== angle;
	
	if (dirty) {
		this.position    = position;
		this.orientation = orientation;
		this.angle       = angle;
		this.valid       = true;
		
		// Update the SVG rendering.
		var transform = webmap.svg.createSVGMatrix().translate(this.position.x, this.position.y).rotate(this.angle);
		this.svg.transform.baseVal.initialize(this.svg.transform.baseVal.createSVGTransformFromMatrix(transform));
		this.svg.style.display = this.valid ? "block" : "none";
		
		this.notifyModules("onOdometryUpdate");
	}
}

/// Handle mouse click events.
webmap.Robot.prototype.mouseClick = function(event) {
	event = event || window.event;
	event.stopPropagation();
	event.preventDefault();
	if (this.map) this.map.setSelected(this);
}