Code Samples

<rover.pde>

EnLiST-Rover mars rover kinematics simulator
Code located at https://github.com/0xdec/EnLiST-Rover
// Defines
public static final boolean STATIONARY  = false;
public static final boolean STEERABLE   = true;
public static final boolean NORMAL      = false;
public static final boolean REVERSE     = true;

public class Rover {
  // Wheel array
  Wheel[] wheels = new Wheel[6];
  Mast mast;
  private float bodyWidth, bodyHeight;

  Rover(float w, float h) {
    bodyWidth = w / 2;
    bodyHeight = h / 2;

    // Wheel declarations
    wheels[0] = new Wheel(STEERABLE,  REVERSE, -bodyWidth * 1.5, bodyHeight);
    wheels[1] = new Wheel(STEERABLE,  NORMAL,   bodyWidth * 1.5, bodyHeight);
    wheels[2] = new Wheel(STATIONARY, REVERSE, -bodyWidth * 1.5, 0);
    wheels[3] = new Wheel(STATIONARY, NORMAL,   bodyWidth * 1.5, 0);
    wheels[4] = new Wheel(STEERABLE,  REVERSE, -bodyWidth * 1.5, -bodyHeight);
    wheels[5] = new Wheel(STEERABLE,  NORMAL,   bodyWidth * 1.5, -bodyHeight);

    mast = new Mast(0, bodyHeight * 0.875);
  }

  void draw() {
    // Center the matrix (from now on, the origin is at (640, 360))
    pushMatrix();
    translate(width / 2, height / 2);

    for (Rover.Wheel wheel : wheels) {
      // Update all wheels
      wheel.update();

      if (wheel.isPressed) {
        wheel.drawRadius();
      }
    }

    if (driveMode) {
      drawBody();
      mast.draw();
      if (drawRoverRotation.pressed()) {
        drawRotation();
      }
    } else {
      if (drawRoverRadius.pressed()) {
        drawRadius();
      }
      drawBody();
      mast.draw();
      if (drawRoverVelocity.pressed()) {
        drawVelocity();
      }
    }

    // Draw all wheels
    for (Rover.Wheel wheel : wheels) {
      wheel.draw();
    }

    popMatrix();
  }

  boolean over() {
    if ((mouseX > width / 2 - bodyWidth) && (mouseX < width / 2 + bodyWidth) &&
        (mouseY > height / 2 - bodyHeight) && (mouseY < height / 2 + bodyHeight)) {
      return true;
    } else {
      return false;
    }
  }

  // Draws a circle representing the path that the center of the rover will take
  private void drawRadius() {
    noFill();
    stroke(0, 0, 0, 200);
    ellipseMode(RADIUS);
    point(0, 0);

    if (X1Stick == 0) {
      // If we're going straight, just draw a line
      line(0, -height / 2, 0, height / 2);
    } else {
      // If not, draw a circle, and a point at the center of that circle
      point(-roverRadius, 0);
      ellipse(-roverRadius, 0, abs(roverRadius), abs(roverRadius));
    }
  }

  // Draws a vector representing the velocity of the center of the rover
  private void drawVelocity() {
    // Don't draw anything if the rover isn't moving
    if (roverVelocity != 0) {
      stroke(0, 0, 0, 255);
      line(0, 0, 0, -roverVelocity * bodyHeight * 0.75);
      line(-10, -roverVelocity * bodyHeight * 0.75, 10, -roverVelocity * bodyHeight * 0.75);
    }
  }

  // Draws a curved vector representing the rover's rotation about its center
  private void drawRotation() {
    noFill();
    stroke(0, 0, 0, 255);
    ellipseMode(RADIUS);

    point(0, 0);

    // Only necessary because Processing does not allow negative angles in arcs
    if (roverRotation < 0) {
      arc(0, 0, 50, 50, 0, -roverRotation * TWO_PI);
    } else {
      arc(0, 0, 50, 50, TWO_PI - roverRotation * TWO_PI, TWO_PI);
    }

    // Only draw the tip of the arrow if the rover is actually turning
    if (roverRotation != 0) {
      pushMatrix();
      rotate(-roverRotation * TWO_PI);
      line(45, 0, 55, 0);
      popMatrix();
    }
  }

  // Draws the body of the rover as a stationary, semi-translucent rectangle
  private void drawBody() {
    fill(255, 255, 255, 127);
    if (over()) {
      stroke(0, 0, 0, 255);
    } else {
      stroke(0, 0, 0, 127);
    }

    rectMode(RADIUS);
    rect(0, 0, bodyWidth, bodyHeight, 5);
  }

  // Wheel class. Stores data about each wheel, and is also responsible for
  // calculating all required numbers. It also takes care of drawing itself.
  class Wheel {
    // Determines whether this wheel is capable of rotating or not and whether
    // or not it is mounted backwards.
    private boolean isSteerable, isReverse;
    // The horizontal and vertical offsets from the center of the rover and the
    // angle the wheel is mounted at.
    private float xPos, yPos, centerAngle;
    // The center of the circle around which the wheel will travel, relative to
    // the wheel.
    private float centerX, centerY;
    // The three variables that define the motion of the wheel
    private float _angle, _radius, _velocity;
    // Flag to set whether or not the wheel has been pressed
    private boolean isPressed = true;

    private float _width, _height;

    // Constructor. Takes inputs to determine whether or not it is steerable as
    // well as its horizontal and vertical offsets from the center of the rover.
    Wheel(boolean s, boolean r, float x, float y) {
      isSteerable = s;
      isReverse = r;
      xPos = x;
      yPos = y;
      _width = bodyWidth / 5;
      _height = bodyHeight / 5;

      // Calculate the angle of the wheel's home position. This corresponds to
      // 0 degrees on the servo.
      centerAngle = atan2(yPos, xPos);
    }

    // Updates all the wheel variables
    void update() {
      center();
      angle();
      radius();
      velocity();
    }

    // Calls all the required draw functions
    void draw() {
      pushMatrix();
      translate(xPos, -yPos);
      rotate(-_angle);

      drawWheel();
      stroke(0, 0, 0);
      if (isPressed) {
        point(0, 0);
        drawVelocity();
      }

      popMatrix();

      if (isPressed) {
        fill(48, 48, 48);
        textAlign(CENTER, CENTER);

        text(String.format("%.0f", degrees(servoAngle())) + (char)0x00B0, xPos, -(yPos + _height + 20));
        text(String.format("%.0f", wheelVelocity()), xPos, -yPos + _height + 16);

        textAlign(LEFT, CENTER);
      }
    }

    void set(boolean bState) {
      isPressed = bState;
    }

    void toggle() {
      isPressed = !isPressed;
    }


    // Returns a boolean telling whether or not the mouse is over the wheel
    boolean over() {
      radius();  // Make sure the angle calculation is up-to-date

      // Do some fancy-ass math
      float xDistance  = float(mouseX) - (width / 2) - xPos;
      float yDistance  = float(mouseY) - (height / 2) + yPos;
      float hypotenuse = sqrt(sq(xDistance) + sq(yDistance));
      float theta      = _angle + atan2(yDistance, xDistance);

      if ((abs(hypotenuse * cos(theta)) < _height) &&
          (abs(hypotenuse * sin(theta)) < _width)) {
        return true;
      } else {
        return false;
      }
    }

    float wheelVelocity() {
      float wv = _velocity * maxSpeedValue;
      if (wv == -0) {
        wv = 0;
      }

      return wv;
    }

    // Converts the wheel angle to a servo position between -90 and 90 degrees
    float servoAngle() {
      float servoPosition = 0;
      if (isSteerable) {
        servoPosition = _angle - centerAngle;

        // Bound anything that is too large or too small
        if ((xPos * yPos) < 0) {
          servoPosition -= PI;
          if (servoPosition < -PI) {
            servoPosition += TWO_PI;
          }
        } else if (servoPosition > PI){
          servoPosition -= TWO_PI;
        }
      }

      servoPosition += HALF_PI;
      servoPosition = abs(servoPosition);

      return servoPosition;
    }

    // Calculates the horizontal and vertical distances from the center of
    // rotation of the wheel to the center of the wheel.
    private void center() {
      centerX = roverRadius + xPos;
      centerY = yPos;
    }

    // Calculates the angle for the wheel in radians using the global variables
    private float angle() {
      if (driveMode) {
        if (!isSteerable) {
          _angle = HALF_PI;
        } else if (xPos > 0) {
          _angle = centerAngle + HALF_PI;
        } else {
          _angle = centerAngle - HALF_PI;
        }
      } else {
        if (!isSteerable) {
          _angle = HALF_PI;
        } else if (roverRadius > 0) {
          _angle = atan2(centerY, centerX) + HALF_PI;
        } else {
          _angle = atan2(centerY, centerX) - HALF_PI;
        }
      }

      // If the wheel is reversed, reverse the angle
      if (isReverse) {
        _angle += PI;
      }

      // Bound the angle to +/- 2PI
      _angle %= TWO_PI; // TODO: fix this

      return _angle;
    }

    // Calculates the wheel radius using an arbitrary/hypothetical input value
    private float radius(float r) {
      return sqrt(sq(r + xPos) + sq(yPos));
    }

    // Calculates the wheel radius using the global variables
    private float radius() {
      _radius = sqrt(sq(roverRadius + xPos) + sq(yPos));

      return _radius;
    }

    // Calculates the wheel velocity using the global variables
    private float velocity() {
      if (driveMode) {
        _velocity = roverRotation * _radius / maxRadius(roverRadius);
      } else {
        _velocity = _radius * roverVelocity / abs(roverRadius);

        if (isReverse) {
          _velocity *= -1;
        }

        // Check for situations where the wheel should turn in the opposite
        // direction than expected. This occurs when the wheel is
        // non-steerable or has a y-position of 0 and its wheel radius is
        // greater than the global rover radius.
        //
        // TODO: make this work with all wheels, not just non-steerable ones
        if ((!isSteerable || (yPos == 0)) &&
            (Math.signum(roverRadius) * (roverRadius + xPos) < 0)) {
          _velocity *= -1;
        }
      }

      return _velocity;
    }

    // Draws a circle representing the actual path this wheel will take
    void drawRadius() {
      noFill();
      stroke(127, 127, 127, 127);
      ellipseMode(RADIUS);

      if (!driveMode && X1Stick == 0) {
        // If we're going straight, just draw a line
        line(xPos, -height / 2, xPos, height / 2);
      } else {
        // If not, draw a circle, and a point at the center of that circle
        point(-roverRadius, 0);
        ellipse(-roverRadius, 0, _radius, _radius);
      }
    }

    // Draws a vector that represents the velocity of the wheel
    private void drawVelocity() {
      // Don't draw anything if the wheel isn't turning
      if (_velocity != 0) {
        stroke(255, 255, 255);
        line(0, 0, _velocity * _height * 0.75, 0);
        line(_velocity * _height * 0.75, -2, _velocity * _height * 0.75, 2);
      }
    }

    // Draws the actual wheel itself
    private void drawWheel() {
      fill(48, 48, 48, 239);
      if (over()) {
        stroke(0, 0, 0, 255);
      } else {
        stroke(32, 32, 32, 239);
      }
      rectMode(RADIUS);
      rect(0, 0, _height, _width, _width / 2);

      // Show an indicator on the outside of the wheel
      if (drawWheelIndicators.pressed()) {
        stroke(16, 16, 16, 239);
        line(_height - 10, _width + 5, -_height + 10, _width + 5);
      }
    }
  }

  // Draws the camera mast on the front of the rover
  class Mast {
    // The horizontal and vertical offsets from the center of the rover
    private float xPos, yPos;

    Mast(float x, float y) {
      xPos = x;
      yPos = y;
    }

    void draw() {
      pushMatrix();
      translate(xPos, -yPos);

      if (mastMode) {
        float increment = X2Stick * PI / 100;
        if (abs(mastPanAngle + increment) <= PI) {
          mastPanAngle += increment;
        }
      } else {
        mastPanAngle = X2Stick * PI;
      }
      rotate(mastPanAngle);

      fill(48, 48, 48, 255);
      noStroke();
      rectMode(RADIUS);
      rect(0, 0, bodyWidth / 2, bodyWidth / 8, bodyWidth / 16);
      rect(0, bodyWidth / 8, bodyWidth / 4, bodyWidth / 16, bodyWidth / 16);

      popMatrix();
    }
  }
}

<element.js>

draft.js primitive element class
Code located at https://github.com/0xdec/draft.js/tree/dev
// let _type = new WeakMap();
// let _id = new WeakMap();

draft.Element = class Element {
  constructor(name) {
    // Make sure _metadata and _properties are initialized
    this._metadata = {
      name: name
    };
    this._properties = {};
    this.transforms = [];

    // HACK:0 use this.constructor.name to get an element's type. Requires all
    // subclasses to have a defined constructor.
    for (let type in draft) {
      if (this.constructor === draft[type]) {
        this._type = type.toLowerCase();
        // _type.set(this, type.toLowerCase());
        break;
      }
    }
    if (!this._type) {
      this._type = this.constructor.name;
    }
    // console.log('NAME:', this.constructor.name);
  }

  /* static inherit(source, addSuper) {
    draft.inherit(this, source, addSuper);
  } */

  static extend(name, config, parent) {
    var Class = class extends this {
      constructor() {
        super();

        /* if ('construct' in config) {
          super();
          // config.construct.call(this, ...args);
        } else {
          super(...args);
        } */
      }
      foo() {
        return 'foo';
      }
      get getter() {
        return 'get';
      }
    };

    Object.defineProperty(Class, 'name', {
      configurable: true,
      value: name
    });

    var mixin = (destination, source) => {
      for (let prop in source) {
        if (prop === 'static') {
          mixin(destination.constructor, source.static);
        } else if (prop !== 'construct') {
          let descriptor = Object.getOwnPropertyDescriptor(source, prop);
          descriptor.enumerable = false;

          // console.info(prop, descriptor);

          Object.defineProperty(destination, prop, descriptor);
        }
      }
    };

    mixin(Class.prototype, config);

    // console.log(`${name}:`, Class);

    if (parent !== null) {
      (parent || draft.Group).mixin({
        [name.toLowerCase()](...args) {
          var instance = this.append(new Class());
          if ('construct' in config) {
            config.construct.call(instance, ...args);
          }
          return instance;
        }
      });
    }

    return Class;
  }

  static extendSchema(name, config, parent) {
    var Class = class extends this {};

    Object.defineProperty(Class, 'name', {
      configurable: true,
      value: name
    });
    Class.schema = draft.proxy({
      has: {}
    });

    var merge = (destination, source) => {
      for (let prop in source) {
        let val = source[prop];

        if (val === null) {
          delete destination[prop];
          delete Class.schema.has[prop];
        } else if (val.type === 'object') {
          merge(destination[prop] || (destination[prop] = {}), val.properties);
          Class.schema.has[prop] = val.alias || [];
        } else {
          destination[prop] = val;
        }
      }
    };
    if (config.schema) {
      merge(Class.schema, this.schema || {});
      merge(Class.schema, config.schema);
    }

    var mixin = (destination, source) => {
      for (let prop in source) {
        let descriptor = Object.getOwnPropertyDescriptor(source, prop);
        descriptor.enumerable = false;
        Object.defineProperty(destination, prop, descriptor);
      }
    };
    if (typeof config.methods == 'object') {
      mixin(Class.prototype, config.methods);
    }

    console.log(`${name}:`, Class, 'schema:', Class.schema);

    if (parent !== null) {
      (parent || draft.Group).mixin({
        [name.substr(0, 1).toLowerCase() + name.substr(1)]() {
          var instance = this.append(new Class());
          return instance;
        }
      });
    }

    return Class;
  }

  static mixin(source) {
    draft.mixin(this, source);
  }

  get type() {
    return this._type;
    // return _type.get(this);
  }

  get id() {
    return this._id;
    // return _id.get(this);
  }

  // Construct a unique ID from the element's type and ID
  get domID() {
    var id = String(this.id);

    // TODO: make the domID digit length configurable
    return `${this.type}_${'0'.repeat(Math.max(4 - id.length, 0))}${id}`;
  }

  get meta() {
    return this._metadata;
  }

  prop(prop, val) {
    if (prop === undefined) {
      // Act as a full properties getter if prop is undefined
      return this._properties;
    } else if (prop === null) {
      // Delete all properties if prop is null
      this._properties = {};
    } else if (typeof prop == 'string') {
      var props = draft.proxy(this._properties);

      if (val === undefined) {
        // Act as an individual property getter if val is undefined
        // TODO: do a fuzzy-find? For example, el.prop('width') would match
        // el._properties.size.width if el._properties.width is undefined
        return prop in props ? props[prop] : null;
      } else if (val === null) {
        // Delete the property if val is null
        delete props[prop];
        this.fire('change', [prop, val]);
      } else if (props[prop] !== val) {
        // Act as an individual property setter if both prop and val are defined

        // TODO: let properties be objects (don't stringify)
        if (val.type === 'color') {
          val = String(val);
        }

        props[prop] = val;
        this.fire('change', [prop, val]);
      }
    } else if (typeof prop == 'object') {
      // Act as a getter if prop is an object with only null values.
      // Act as a setter if prop is an object with at least one non-null value.
      let setter = false;

      for (let p in prop) {
        // Get this._properties[p] and save it to prop[p]
        prop[p] = this.prop(p, prop[p]);
        // If the returned value is an object, prop[p] is non-null, so act like
        // a setter.
        setter = setter || typeof prop[p] == 'object';
      }

      return setter ? this : prop;
    }

    // Chainable if 'this' is returned
    return this;
  }

  propSchema(prop, val) {
    var schema = this.constructor.schema;

    for (let name in schema.has) {
      if (prop === name || schema.has[name].includes(prop)) {
        return this.prop(name, val);
      }
    }
  }

  // Get/set the element's default length unit
  unit(unit) {
    return this.prop('unit', unit);
  }

  // TODO: use rest (...blacklist) for multiple blacklist items?
  stringify(blacklist) {
    var replacer;

    if (Array.isArray(blacklist)) {
      replacer = function(key, val) {
        if (blacklist.includes(key)) {
          return undefined;
        }

        return val;
      };
    } else if (blacklist instanceof RegExp) {
      replacer = function(key, val) {
        if (blacklist.test(key)) {
          return undefined;
        }

        return val;
      };
    }

    return JSON.stringify(this, replacer, 2);
  }

  toJSON() {
    return {
      type: this.type,
      id: this.id,
      properties: this.prop(),
      children: this.children
    };
  }
};

draft.Element.mixin([
  'event',
  'transform'
]);

<svg.js>

draft.js SVG renderer
Code located at https://github.com/0xdec/draft.js/tree/dev
let _svg = new WeakMap();
let _maxWidth = new WeakMap();
let _maxHeight = new WeakMap();

draft.mixins.svg = {
  svg(width, height) {
    width = width ? draft.newLength(width) : _maxWidth.get(this);
    height = height ? draft.newLength(height) : _maxHeight.get(this);

    var calcX = function(element) {
      return (element.prop('x') || 0) - element.prop('width') / 2;
    };
    var calcY = function(element) {
      return -(element.prop('y') || 0) - element.prop('height') / 2;
    };

    var domPrefix = `${this.doc.domID}:${this.domID}:svg`;
    var domID = function(element) {
      return `${domPrefix}:${element.domID}`;
    };

    _maxWidth.set(this, width || this.prop('width'));
    _maxHeight.set(this, height || this.prop('height'));

    if (!_svg.has(this)) {
      const NS = 'http://www.w3.org/2000/svg';
      // const XMLNS = 'http://www.w3.org/2000/xmlns/';
      // const XLINK = 'http://www.w3.org/1999/xlink';
      const VERSION = '1.1';

      var render = function(element) {
        // console.info('rendering svg:', element.domID);

        // TODO: separate listener for each property?
        var node, listener;
        var type = element.type;
        var create = svgType => document.createElementNS(NS, svgType);

        if (element instanceof draft.Group) {
          node = create('g');

          let renderChild = function(child) {
            let childNode = render(child);
            if (childNode) {
              node.appendChild(childNode);
            }
          };

          for (let child of element.children) {
            renderChild(child);
          }

          element.on('add', renderChild);

          element.on('remove', function(child) {
            node.removeChild(document.getElementByID(domID(child)));
          });

          type = 'group';
        }

        switch (type) {
          case 'line':
            node = create('line');

            listener = function(prop, val) {
              var size;

              switch (prop) {
                case 'y':
                  val *= -1;
                  size = 0;
                  break;
                case 'width':
                  val = this.target.prop('x');
                  // Falls through
                case 'x':
                  prop = 'x';
                  size = this.target.prop('width') / 2;
                  break;
                default:
                  return;
              }

              node.setAttribute(`${prop}1`, val - size);
              node.setAttribute(`${prop}2`, val + size);
            };

            break;
          case 'group':
          case 'rectangle':
            node = node || create('rect');

            listener = function(prop, val) {
              var link;

              switch (prop) {
                case 'width':
                  link = 'x';
                  // Falls through
                case 'height':
                  link = link || 'y';
                  listener.call(this, link, this.target.prop(link));
                  break;
                case 'y':
                  link = 'height';
                  val *= -1;
                  // Falls through
                case 'x':
                  link = link || 'width';
                  val -= this.target.prop(link) / 2;
                  break;
                default:
                  return;
              }

              node.setAttribute(prop, val);
            };

            break;
          case 'square':
            node = node || create('rect');

            listener = function(prop, val) {
              switch (prop) {
                case 'width':
                  node.setAttribute('height', val);

                  for (let link of ['x', 'y']) {
                    listener.call(this, link, this.target.prop(link));
                  }

                  break;
                case 'y':
                  val *= -1;
                  // Falls through
                case 'x':
                  val -= this.target.prop('width') / 2;
                  break;
                default:
                  return;
              }

              node.setAttribute(prop, val);
            };

            break;
          case 'ellipse':
            node = create('ellipse');

            listener = function(prop, val) {
              switch (prop) {
                case 'width':
                  prop = 'rx';
                  val.value /= 2;
                  break;
                case 'height':
                  prop = 'ry';
                  val.value /= 2;
                  break;
                case 'y':
                  val.value *= -1;
                  // Falls through
                case 'x':
                  prop = `c${prop}`;
                  break;
                default:
                  return;
              }

              node.setAttribute(prop, val);
            };

            break;
          case 'circle':
            node = create('circle');

            listener = function(prop, val) {
              switch (prop) {
                case 'diameter':
                case 'width':
                  prop = 'r';
                  val.value /= 2;
                  break;
                case 'y':
                  val.value *= -1;
                  // Falls through
                case 'x':
                  prop = `c${prop}`;
                  break;
                default:
                  return;
              }

              node.setAttribute(prop, val);
            };

            break;
          default:
            return;
        }

        // TODO: support all elements
        node.id = domID(element);

        let hasStyle = [];
        for (let style of ['fill', 'stroke']) {
          if (style in element) {
            hasStyle.push(style);
          }
        }

        if (hasStyle.length) {
          let styleListener = function(prop, val) {
            prop = prop.replace('.color', '').replace('.', '-');

            var color = /^(fill|stroke)(-opacity)?$/;
            var stroke = /^stroke-(width)?$/;

            if (color.test(prop) || stroke.test(prop)) {
              node.setAttribute(prop, val);
            }
          };

          for (let style of hasStyle) {
            for (let prop of ['color', 'opacity', 'width']) {
              prop = `${style}.${prop}`;
              let val = element.prop(prop) || draft.defaults[prop];

              styleListener(prop, val);
            }
          }

          element.on('change', styleListener);
        }

        for (let prop in element.prop()) {
          listener.apply({target: element}, [prop, element.prop(prop)]);
        }

        element.on('change', listener);

        node.addEventListener('click', function() {
          element.fire('click');
        });

        return node;
      };

      var svg = document.createElementNS(NS, 'svg');
      _svg.set(this, svg);
      svg.setAttribute('xmlns', NS);
      svg.setAttribute('version', VERSION);
      // svg.setAttributeNS(XMLNS, 'xmlns:xlink', XLINK);

      svg.id = domID(this);

      var listener = function(prop) {
        if (prop === 'width' || prop === 'height') {
          let targetWidth = this.target.prop('width');
          let targetHeight = this.target.prop('height');

          // 1 SVG user unit = 1px
          svg.setAttribute('viewBox', [
            calcX(this.target), calcY(this.target),
            targetWidth, targetHeight
          ].map(val => val.valueOf()).join(' '));

          let zoom = Math.min(
            _maxWidth.get(this.target) / targetWidth,
            _maxHeight.get(this.target) / targetHeight
          );

          let svgWidth = targetWidth * zoom;
          let svgHeight = targetHeight * zoom;

          _svg.get(this.target).setAttribute('width', svgWidth);
          _svg.get(this.target).setAttribute('height', svgHeight);

          // console.info('aspect ratio:', this.target.aspectRatio);
        }
      };

      listener.apply({target: this}, ['width']);
      listener.apply({target: this}, ['height']);

      this.on('change', listener);

      svg.appendChild(render(this.parent));
    }

    return _svg.get(this);
  }
};

<i2c.c>

Self-Playing Clarinet ARM I2C driver
Code located at https://gitlab.com/0xdec/self-playing-clarinet
#include "i2c.h"

static void I2C_wait(void);
static uint8_t I2C_status(void);
static void I2C_ack(void);
static void I2C_nack(void);
static void I2C_start(void);
static void I2C_address(uint8_t address);
static uint8_t I2C_read_data(void);
static void I2C_write_data(uint8_t data);
static void I2C_stop(void);

uint8_t mode      = 0;
uint8_t available = 0;
uint8_t buffer[256];

// Initialize I2C interface
void I2C_init(void) {
  // Select pin function SCL (sec 7.4.11)
  *iocon_register[I2C_SCL_PORT][I2C_SCL_PIN] &= ~BIT(1);
  *iocon_register[I2C_SCL_PORT][I2C_SCL_PIN] |=  BIT(0);
  // Select pin function SDA (sec 7.4.12)
  *iocon_register[I2C_SDA_PORT][I2C_SDA_PIN] &= ~BIT(1);
  *iocon_register[I2C_SDA_PORT][I2C_SDA_PIN] |=  BIT(0);

  // Enable clock for I2C (sec 3.5.14)
  LPC_SYSCON->SYSAHBCLKCTRL |= BIT(5);
  // I2C reset de-asserted (sec 3.5.2)
  LPC_SYSCON->PRESETCTRL |= BIT(1);

  // High duty cycle register for PCLK=48MHz (sec 15.7.5)
  LPC_I2C->SCLH = 0x003C;
  // Low duty cycle register for PCLK=48MHz (sec 15.7.5)
  LPC_I2C->SCLL = 0x003C;
  // Enable I2C interface (sec 15.7.1)
  LPC_I2C->CONSET |= BIT(6);
}

// Transmit data via I2C
uint8_t I2C_transmit(uint8_t address, uint8_t length, uint8_t* data) {
  // Master transmit mode
  mode      = 0;
  uint8_t i = 0;

  I2C_start();

  while (true) {
    // Get status
    uint8_t status = I2C_status();
		// UART_transmit(status);

    if (status == 0x08 || status == 0x10) {
      /*
       * 0x08: A START condition has been transmitted.
       * 0x10: A repeated START condition has been transmitted.
       *  - Load SLA+W into DAT
       *  - Clear STA
       */

      // Transmit slave address and data direction bit
      I2C_address(address);

      // Clear STA bit (sec 15.7.6)
      LPC_I2C->CONCLR = BIT(5);
    } else if (status == 0x18 || status == 0x28) {
      /*
       * 0x18: SLA+W has been transmitted, ACK has been received.
       * 0x28: Data byte in DAT has been transmitted, ACK has been received.
       *  - Load data byte into DAT
       *    or
       *  - Transmit STOP condition
       */

      if (i < length) {
        // Transmit data byte
        I2C_write_data(data[i++]);
      } else {
        I2C_stop();
        return 0x00;
      }
    } else if (status == 0x20 || status == 0x30) {
      /*
       * 0x20: SLA+W has been transmitted, NOT ACK has been received.
       * 0x30: Data byte in DAT has been transmitted, NOT ACK has been received.
       *  - Transmit STOP condition
       */

      I2C_stop();
      return 0x00;
    } else if (status == 0x38) {
      /*
       * 0x38: Arbitration lost in SLA+R/W or data bytes.
       * 0x00: Bus error has occurred during an I2C serial transfer.
       *  - Exit
       */

      return status;
    } else if (status == 0x00) {
      /*
       * 0x00: Bus error durring MST, due to an illegal START or STOP condition.
       *  - Set STO
       *  - Clear SI
       *  - Exit
       */

      I2C_stop();
      return status;
    }

    // Clear SI bit (sec 15.7.6)
    LPC_I2C->CONCLR = BIT(3);
  }
}

// Request data via I2C
uint8_t I2C_request(uint8_t address, uint8_t length) {
  // Master receive mode
  mode = 1;
  available = 0;

  I2C_start();

  while (true) {
    // Get status
    uint8_t status = I2C_status();
		// UART_transmit(status);

    if (status == 0x08 || status == 0x10) {
      /*
       * 0x08: A START condition has been transmitted.
       * 0x10: A repeated START condition has been transmitted.
       *  - Load SLA+R into DAT
       */

      // Transmit slave address and data direction bit
      I2C_address(address);
    } else if (status == 0x38) {
      /*
       * 0x38: Arbitration lost in NOT ACK bit.
       *  - Exit
       */

      return status;
    } else if (status == 0x40) {
      /*
       * 0x40: SLA+R has been transmitted, ACK has been received.
       *  - Set AA
       */

      buffer[available++] = I2C_read_data();

      if (available < length) {
        I2C_ack();
      } else {
        I2C_nack();
      }
    } else if (status == 0x48) {
      /*
       * 0x48: SLA+R has been transmitted, NOT ACK has been received.
       *  - Transmit STOP condition
       */

      I2C_stop();
      return 0x00;
    } else if (status == 0x50) {
      /*
       * 0x50: Data byte has been received, ACK has been returned.
       *  - Read data byte
       *  - Set AA
       *    or
       *  - Clear AA
       */

      buffer[available++] = I2C_read_data();

      if (available < length) {
        I2C_ack();
      } else {
        I2C_nack();
      }
    } else if (status == 0x58) {
      /*
       * 0x58: Data byte has been received, NOT ACK has been returned.
       *  - Read data byte
       *  - Transmit STOP condition
       */

      buffer[available++] = I2C_read_data();
      I2C_stop();
      return 0x00;
    } else if (status == 0x00) {
      /*
       * 0x00: Bus error durring MST, due to an illegal START or STOP condition.
       *  - Set STO
       *  - Clear SI
       *  - Exit
       */

      I2C_stop();
      return status;
    }

    // Clear SI bit (sec 15.7.6)
    LPC_I2C->CONCLR = BIT(3);
  }
}

// Get number of received data bytes
uint8_t I2C_available(void) {
  return available;
}

// Read a received data byte
uint8_t I2C_read(uint8_t index) {
  return available && index < available ? buffer[index] : 0;
}



// Wait for SI
static void I2C_wait(void) {
  // Wait for SI bit (sec 15.10.1 table 236)
  while (!(LPC_I2C->CONSET & BIT(3)));
}

// Get I2C status
static uint8_t I2C_status(void) {
  uint8_t status;

  // 0xF8: No relevant state information available, SI = 0.
  do {
    status = LPC_I2C->STAT & 0xF8;
  } while (status == 0xF8);

  I2C_wait();

  return status;
}

static void I2C_ack(void) {
  // Set AA bit (sec 15.7.1)
  LPC_I2C->CONSET = BIT(2);
}

static void I2C_nack(void) {
  // Clear AA bit (sec 15.7.1)
  LPC_I2C->CONCLR = BIT(2);
}

// Transmit START condition
static void I2C_start(void) {
  // Set STA bit (sec 15.7.1)
  LPC_I2C->CONSET = BIT(5);
}

// Load SLA+R/W
static void I2C_address(uint8_t address) {
  I2C_write_data((address << 1) | (mode % 2));
}

// Read data byte
static uint8_t I2C_read_data(void) {
  return LPC_I2C->DAT;
}

// Load data byte
static void I2C_write_data(uint8_t data) {
  // Send data byte (sec 15.7.3)
  LPC_I2C->DAT = data;
  // Set AA bit (sec 15.7.1)
  LPC_I2C->CONSET = BIT(2);
}

// Transmit STOP condition
static void I2C_stop(void) {
  // Set STO and AA bits (sec 15.7.1)
  LPC_I2C->CONSET = BIT(4) | BIT(2);
  // Clear SI bit (sec 15.7.6)
  LPC_I2C->CONCLR = BIT(3);
}

<lab4.cpp>

ROS robotic arm control program with inverse kinematics
#include "ros/ros.h"
#include <string>
#include <iostream>
#include <cstdio>
#include <unistd.h>
#include <stdlib.h>
#include <math.h>

// Custom rhino_ros msg and srv
#include <rhino_ros/command.h>
#include <rhino_ros/positions.h>
// Custom lab4 msg
#include <lab4/command.h>

using std::cout;
using std::endl;
using std::cin;
using std::string;
using std::atof;

// Uncomment to enable cout for debugging
//#define DEBUG
//#define DEBUG_VERBOSE

// Deg and rad conversion macros
#define RAD(d) ((d)*M_PI/180)
#define DEG(r) ((r)*180/M_PI)
// #define LOC(a,b,c) acos((pow((b),2)+pow((c),2)-pow((a),2))/(2*(b)*(c)))

// Base frame offsets
#define B_X  11.2
#define B_Y -15.0
#define B_Z  -3.7

// Linkage dimensions
#define d1   24.3
#define a2   23.2
#define a3   23.0
#define a4    1.1
#define d5   16.2

// Syntactic sugar for named motors
#define grip_open()       grip(0) // rhino_ros::rhino_ungrip();
#define grip_close()      grip(1) // rhino_ros::rhino_grip();
#define wrist_rotate(v)   motor(0, (v)) // Motor B
#define wrist_flex(v)     motor(1, (v)) // Motor C
#define elbow_flex(v)     motor(2, (v)) // Motor D
#define shoulder_flex(v)  motor(3, (v)) // Motor E
#define waist_rotate(v)   motor(4, (v)) // Motor F

// ROS publisher and subscriber
ros::Publisher  lab_pub;
ros::Subscriber lab_sub;
// Topic details can be obtained by running "rostopic info /rhino/command"
// Message details can be obtained by running "rosmsg show rhino_ros/command"
rhino_ros::command msg;
// Global position variable, used to transfer msg from callback function to
// wherever it is needed.
boost::array<int,5> positions;
int grip_state;



// Position callback function, for details see lab2.cpp
void position_callback(const rhino_ros::positions::ConstPtr& m) {
  std::copy(m->position.begin(), m->position.begin() + 5, positions.begin());
  grip_state = m->grip;
}

// Wait for a subscriber to latch onto the publisher
void wait_latched() {
  ros::Rate poll_rate(100);

  while (!lab_pub.getNumSubscribers()) {
    poll_rate.sleep();
  }
}
// Wait until ROS is ready for operation
void wait_ready() {
  while (!ros::ok()) {};
}

// Reset the message
void msg_reset() {
  msg.softhome  = 0;
  msg.grip      = 0;
  for (int i = 0; i < 5; i++) {
    msg.destination[i] = 0;
  }
}
// Publish the message once
void msg_send() {
  #ifdef DEBUG_VERBOSE
  cout << msg << " destination" << endl;
  #endif

  lab_pub.publish(msg);
  ros::spinOnce();
}
// Publish the message repeatedly until the position has been reached
void msg_repeat() {
  // High loop rate may result in undesired behaviors
  ros::Rate loop_rate(4);

  while ((msg.destination != positions || msg.grip != grip_state) && ros::ok()) {
    #ifdef DEBUG_VERBOSE
    cout << "positions: ";
    for (int i = 0; i < 5; i++) {
      cout << " " << positions[i];
    }
    cout << endl;
    #endif

    msg_send();

    // "Wait" so that the program runs at rate of loop_rate, 4 Hz
    loop_rate.sleep();
  }
}

// Go Soft Home
void soft_home() {
  #ifdef DEBUG
  cout << "\tgoing soft home" << endl;
  #endif

  wait_ready();

  msg_reset();
  msg.softhome = 1;
  msg_repeat();

  msg.softhome = 0;
}
// Open or close the gripper (Motor A)
void grip(int closed) {
  #ifdef DEBUG
  if (closed) {
    cout << "\tclosing gripper" << endl;
  } else {
    cout << "\topening gripper" << endl;
  }
  #endif

  wait_ready();

  msg.grip = closed;
  msg_send(); // TODO: msg_repeat()?
}
// Move only the given motor to the given encoder value (Motors B-F)
void motor(int motor, int value) {
  #ifdef DEBUG
  cout << "\tmoving motor " << (char)('B' + motor) << " to " << value << endl;
  #endif

  wait_ready();

  msg.destination[motor] = value;
  msg_repeat();
}

// Calculate encoder values for each motor
boost::array<int,5> lab_angles(float theta1, float theta2, float theta3, float theta4, float theta5) {
  boost::array<int,5> encoders;

  // Experimentally derived linear encoder value equations
  encoders[0] = theta5                      * 293 / 90;
  encoders[1] = (theta4 + theta3 + theta2)  * 803 / 90;
  encoders[2] = (theta3 + theta2)           * 777 / 90;
  encoders[3] = (theta2 - 124)              *-775 / 90;
  encoders[4] = theta1                      * 393 / 90;

  cout << "Computed encoder values:" << endl;
  for (int i = 0; i < 5; i++) {
    cout << (char)('B' + i) << ": " << encoders[i] << endl;
  }
  cout << endl;

  return encoders;
}

// Compute the elbow-up inverse kinematics solutions
boost::array<int,5> lab_movex(float xworld, float yworld, float zworld, float pitch, float roll) {
  // Computed joint angles
  double theta[5];

  // Distance in xy from origin to wrist in base frame
  double db;
  // Distance in xyz from origin to wrist in frame 1
  double o1;
  // Angle from horizontal to wrist origin
  double theta_w;
  // Angle from wrist origin to elbow
  double theta_we;
  // Angle between upper and lower arm
  double theta_e;

  // Position structure
  struct pos {
    double x;
    double y;
    double z;
  };
  // Wrist origin W.R.T base frame & frame 1
  pos wr_b, wr_1;

  // Calculate theta5 (wrist angle)
  theta[4] = roll;

  // Calculate theta1 (waist angle)
  theta[0] = DEG(atan2(yworld + B_Y, xworld + B_X));

  // Calculate wrist origin W.R.T. base frame
  db = a4*cos(RAD(pitch)) + d5*sin(RAD(pitch));
  wr_b.x = xworld + B_X - db*cos(RAD(theta[0]));
  wr_b.y = yworld + B_Y - db*sin(RAD(theta[0]));
  wr_b.z = zworld + B_Z - a4*sin(RAD(pitch)) + d5*cos(RAD(pitch));

  // Calculate wrist origin W.R.T. frame 1
  wr_1.x = wr_b.x;
  wr_1.y = wr_b.y;
  wr_1.z = wr_b.z - d1;
  o1 = sqrt(pow(wr_1.x, 2) + pow(wr_1.y, 2) + pow(wr_1.z, 2));

  // Solve the arm triangle
  theta_w   = atan2(wr_1.z, sqrt(pow(wr_1.x, 2) + pow(wr_1.y, 2)));
  theta_we  = acos((pow(o1, 2) + pow(a2, 2) - pow(a3, 2)) / (2 * o1 * a2)); // LOC(a3, o1, a2)
  theta_e   = acos((pow(a2, 2) + pow(a3, 2) - pow(o1, 2)) / (2 * a2 * a3)); // LOC(o1, a2, a3)

  // Calculate theta2 (shoulder flex), theta3 (elbow flex), theta4 (wrist flex)
  theta[1] = DEG(theta_w + theta_we);
  theta[2] = DEG(theta_e) - 180;
  theta[3] = pitch - theta[1] - theta[2];

  // Print calculated angles (TODO: print forward kinematics)
  for (int i = 0; i < 5; i++) {
    cout << "theta" << i + 1 << " = " << theta[i] << " degrees" << endl;
  }

  // Check that your values are good BEFORE sending commands to rhino
  return lab_angles(theta[0], theta[1], theta[2], theta[3], theta[4]);
}

void move(float x, float y, float z, float pitch, float roll) {
  msg.destination = lab_movex(x, y, z, pitch, roll);
  cout << "msg destination:\n" << msg << endl;
  msg_repeat();
}

void command_callback(const lab4::command::ConstPtr& m) {
  cout << "lab56 command:\n" << *m << endl;
  if (m->softhome) {
    soft_home();
  } else {
    grip(m->grip);
    move(m->x, m->y, m->z, m->pitch, m->roll);
  }
}



int main(int argc, char **argv) {
  // Initialization of ROS required for each Node
  ros::init(argc, argv, "lab4");
  // Handler for this node
  ros::NodeHandle nh;
  // Define a publisher to publish to topic "rhino/command" with msg type
  // <rhino_ros::command>
  lab_pub = nh.advertise<rhino_ros::command>("rhino/command", 1000);
  // Define a subscriber to subscribe to topic "rhino/position" with msg type
  // <rhino_ros::positions>
  lab_sub = nh.subscribe<rhino_ros::positions>("rhino/position", 100, position_callback);

  // Wait for a subscriber to latch onto the publisher
  wait_latched();
  // Go soft home
  soft_home();

  // Parse command line arguments
  if (argc == 1) {
    ros::Subscriber sub_command = nh.subscribe<lab4::command>(
      "/rhino/target_position", 100, command_callback);

    while (ros::ok()) {
      ros::spinOnce();
    }
  } else if (argc == 6) {
    // xworld, yworld, zworld, pitch, roll
    float params[5];
    for (int i = 0; i < 5; i++) {
      //~ float f = atof(sf); Function to turn string to float number
      params[i] = atof(argv[i + 1]);
    }

    cout << "xworld: " << params[0] << ", yworld: " << params[1] << ", zworld: " << params[2] << ", pitch: " << params[3] << ", roll: " << params[4] << endl;

    // Check that your theta and encoder destination values are good BEFORE uncommenting two lines below
    move(params[0], params[1], params[2], params[3], params[4]);
  } else {
    cout << "invalid number of inputs" << endl;
    cout << "command should be: rosrun lab4 lab4 xworld yworld zworld pitch roll" << endl;
    return 0;
  }

  // No softhome, therefore the arm stays at the last destination.
  // To softhome, you can publish softhome through a new terminal:
  // rostopic pub /rhino/command rhino_ros/command "destination: [0, 0, 0, 0, 0] grip: 0 softhome: 1"

  return 0;
}

<lab56.cpp>

ROS robotic arm w/ computer vision block picker using OpenCV
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <cmath>
#include <vector>
#include <map>

// Custom lab4 msg
#include <lab4/command.h>

// Uncomment to enable info stream for debugging
//#define DEBUG

#define PIXEL_LOWER  500
#define PIXEL_UPPER  2000
#define BLOCK_HEIGHT 4.2

#define OR     240   // Height / 2
#define OC     320   // Width / 2
#define BETA   7.303 // 111 / 15.2
#define THETA -0.061 // 4.236 deg
#define TX    -9.8
#define TY   -16.2

#define COLOR_OFFSET           0.0
#define COLOR_SATURATION       0.5
#define COLOR_VALUE            0.95
#define GOLDEN_RATIO_CONJUGATE 0.618033988749895



static const std::string OPENCV_WINDOW = "Image window";
ros::Publisher *pub_command_ptr;
using namespace cv;
vector<Vec3b> colorPalette(1, Vec3b(255, 255, 255));
// A vector of centroid points
vector<Point2f> mc;
// A vector of object orientations
vector<float> mo;
bool holding_block = false;



// Convert from HSV to RGB color space
Vec3b hsv2rgb(double h, double s, double v) {
  double r, g, b;

  if (s == 0.0) {
    r = v;
    g = v;
    b = v;
  } else {
    if (h >= 360.0) {
      h = 0.0;
    }

    h /= 60.0;

    int i = h;
    double f = h - i;

    double p = v * (1.0 - s);
    double q = v * (1.0 - (s * f));
    double t = v * (1.0 - (s * (1.0 - f)));

    switch (i) {
      case 0:
        r = v;
        g = t;
        b = p;
        break;
      case 1:
        r = q;
        g = v;
        b = p;
        break;
      case 2:
        r = p;
        g = v;
        b = t;
        break;
      case 3:
        r = p;
        g = q;
        b = v;
        break;
      case 4:
        r = t;
        g = p;
        b = v;
        break;
      case 5:
        r = v;
        g = p;
        b = q;
        break;
    }
  }

  Vec3b color;
  color[2] = r * 255;
  color[1] = g * 255;
  color[0] = b * 255;

  return color;
}

// Given an image column and row, find the position in the world coordinate frame
Point2f pixel2world(int c, int r) {
  float xc = (OR - r) / BETA;
  float yc = (OC - c) / BETA;
  float xw = cos(THETA) * (xc - TX) + sin(THETA) * (yc - TY);
  float yw = cos(THETA) * (yc - TY) - sin(THETA) * (xc - TX);

  return Point2f(xw, yw);
}

/****************************************************
 * Class defined for both Lab5 and Lab6
 ****************************************************/
class ImageConverter {
  // Node handle
  ros::NodeHandle nh_;
  // Initialize cv bridge sub and pub
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub_;
  image_transport::Publisher image_pub_;
public:
  // Constructor
  ImageConverter();

  // Destructor
  ~ImageConverter();

  // Subscriber callback function, will be called when there is a new camera image
  void imageCb(const sensor_msgs::ImageConstPtr& msg);

private:
  /** lab5's function, thresholding the image
   * @param gray_img A grayscale image
   * @return A threshholded image
   */
  Mat thresholdImage(Mat gray_img);

  /** lab5's function, associating image
   * @param bw_img A black and white (binary) image
   * @return An associated image
   */
  Mat associateObjects(Mat bw_img);
};



/****************************************************
 * Functions in class:
 ****************************************************/

// Constructor
ImageConverter::ImageConverter():it_(nh_) {
  // Subscribe to input video feed and publish output video feed
  image_sub_ = it_.subscribe("/cv_camera_node/image_raw", 1,
    &ImageConverter::imageCb, this);
  image_pub_ = it_.advertise("/image_converter/output_video", 1);
  namedWindow(OPENCV_WINDOW);
}
// Destructor
ImageConverter::~ImageConverter() {
  cv::destroyWindow(OPENCV_WINDOW);
}
// Subscriber callback function, will be called when there is a new camera image
void ImageConverter::imageCb(const sensor_msgs::ImageConstPtr& msg) {
  cv_bridge::CvImagePtr cv_ptr;

  try {
    cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
  } catch (cv_bridge::Exception& e) {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
  }

  // Create a grayscale version of the image
  Mat gray_image;
  cvtColor(cv_ptr->image, gray_image, CV_BGR2GRAY);

  // Convert to black and white img, then associate objects
  Mat bw_image = thresholdImage(gray_image);
  Mat associate_image = associateObjects(bw_image);

  // Update GUI Window
  imshow("Image window", cv_ptr->image);
  imshow("gray_scale", gray_image);
  imshow("black and white", bw_image);
  imshow("associate objects", associate_image);
  waitKey(3);

  // Output some video stream
  image_pub_.publish(cv_ptr->toImageMsg());
}

/****************************************************
 * Function for Lab 5
 ****************************************************/
// Take a grayscale image as input and return a thresholded image.
// You will implement your algorithm for thresholding here.
Mat ImageConverter::thresholdImage(Mat gray_img) {
  // Copy input image to a new image
  Mat bw_img = gray_img.clone();

  // Total number of pixels in image
  int totalpixels = gray_img.rows * gray_img.cols;

  // Histogram for the grayscale image
  int h[256] = {0};
  // Probability array for the grayscale image
  float p[256] = {0};
  // Group probabilities
  float q0p;
  float q0 = 0;
  float q1;
  // Mean values (average gray level)
  float u = 0;
  float u0 = 0;
  float u1;
  // Total and between-group variance (psi^2)
  float v;
  float vb;
  // Threshold grayscale value
  int zt;

  // Create histogram
  for (int i = 0; i < totalpixels; i++) {
    uchar graylevel = gray_img.data[i];
    h[graylevel]++;
  }

  // Calculate probability and mean
  for (int z = 0; z < 256; z++) {
    p[z] = (float)h[z] / totalpixels;
    u += p[z] * z;
  }

  // Calculate variance
  for (int z = 0; z < 256; z++) {
    v += p[z] * pow(z - u, 2);
  }

  // Iterative algorithm
  for (int z = 0; z < 256; z++) {
    // Group probabilities
    q0p = q0;
    q0 += p[z];
    q1 = 1 - q0;

    // Mean values
    if (q0 != 0.0) {
      u0 = (q0p * u0 + p[z] * z) / q0;
    }
    if (q1 != 0.0) {
      u1 = (u - q0 * u0) / q1;
    }

    // Between-group variance
    float vb_temp = q0 * pow(u0 - u, 2) + q1 * pow(u1 - u, 2);
    if (vb_temp > vb) {
      vb = vb_temp;
      zt = z;
    }
  }

  // Threshold the image
  for (int i = 0; i < totalpixels; i++) {
    uchar graylevel = gray_img.data[i];
    bw_img.data[i] = graylevel > zt ? 255 : 0;
  }

  return bw_img;
}

/****************************************************
 * Function for Lab 5
 ****************************************************/
// Take a black and white image and find the object in it, returns an
// associated image with different color for each object.
// You will implement your algorithm for rastering here.
Mat ImageConverter::associateObjects(Mat bw_img) {
  // Function will return this image
  Mat associate_img = Mat::zeros(bw_img.size(), CV_8UC3);

  // Number of rows and colums in image
  int height = bw_img.rows;
  int width  = bw_img.cols;

  int labelNum = 1;
  int* labels = new int[height * width / 2];
  int** equiv = new int*[height * width / 2];
  for (int i = 0; i < height * width / 2; i++) {
    equiv[i] = &labels[i];
  }
  int** pixelLabel = new int*[height];

  // First raster scan
  // Assign a label number to each pixel
  for (int row = 0; row < height; row++) {
    // Initialize an array of labels
    pixelLabel[row] = new int[width];

    for (int col = 0; col < width; col++) {
      // Pixel to object label - background (255): -1, foreground/object (0): 0
      pixelLabel[row][col] = bw_img.data[row * width + col] ? -1 : 0;
      int label = pixelLabel[row][col];

      if (label == 0) {
        int left  = col ? pixelLabel[row][col - 1] : -1;
        int above = row ? pixelLabel[row - 1][col] : -1;

        if (left == -1 && above == -1) {
          // Both left and above are background
          pixelLabel[row][col] = labelNum;
          labels[labelNum] = labelNum;
          labelNum++;
        } else if (left >= 0 && above == -1) {
          // Left is object and above is background
          pixelLabel[row][col] = left;
        } else if (left == -1 && above >= 0) {
          // Left is background and above is object
          pixelLabel[row][col] = above;
        } else {
          // Both left and above are object
          int smaller = fmin(*equiv[left], *equiv[above]);
          pixelLabel[row][col] = smaller;

          int minLabel = smaller == *equiv[left] ? left : above;
          int maxLabel = minLabel == above ? left : above;

          *equiv[maxLabel] = *equiv[minLabel];
          equiv[maxLabel]  = equiv[minLabel];
        }
      }
    }
  }

  // Count the number of pixels in each object
  int* pixelCount = new int[labelNum + 1];
  for (int i = 0; i <= labelNum; i++) {
    pixelCount[i] = 0;
  }

  // Second raster scan
  // Assign the same unique label to all pixels in the same object
  for (int row = 0; row < height; row++) {
    for (int col = 0; col < width; col++) {
      int label = pixelLabel[row][col];
      label = label >= 0 ? *equiv[label] : 0;
      pixelLabel[row][col] = label;
      pixelCount[label]++;
    }
  }

  // Calculate the number of legitimate objects in the image
  std::map<int, int> objects;
  double h = COLOR_OFFSET + GOLDEN_RATIO_CONJUGATE * (colorPalette.size() - 1);

  for (int i = 1; i <= labelNum; i++) {
    if (pixelCount[i] > PIXEL_LOWER && pixelCount[i] < PIXEL_UPPER) {
      int numObjects = objects.size() + 1;
      objects[i] = numObjects;

      // Generate distinct colors using the golden ratio
      // devmag.org.za/2012/07/29/how-to-choose-colours-procedurally-algorithms
      if (colorPalette.size() <= numObjects) {
        h += GOLDEN_RATIO_CONJUGATE;
        colorPalette.push_back(hsv2rgb(fmod(h, 1) * 360, COLOR_SATURATION,
          COLOR_VALUE));
      }
    }
  }
  #ifdef DEBUG
  ROS_INFO_STREAM(objects.size() << " objects in image");
  #endif

  // Clear the centroid and orientation vectors
  mc.clear();
  mo.clear();

  if (objects.size()) {
    int*   m00 = new int[objects.size()];
    int*   m10 = new int[objects.size()];
    int*   m01 = new int[objects.size()];
    float* c11 = new float[objects.size()];
    float* c20 = new float[objects.size()];
    float* c02 = new float[objects.size()];

    // Initialize arrays to 0
    for (int i = 0; i < objects.size(); i++) {
      m00[i] = 0;
      m10[i] = 0;
      m01[i] = 0;
      c11[i] = 0;
      c20[i] = 0;
      c02[i] = 0;
    }

    // Third raster scan
    // Assign a unique color to each object
    for (int row = 0; row < height; row++) {
      for (int col = 0; col < width; col++) {
        int label = 0;
        std::map<int, int>::iterator lookup = objects.find(pixelLabel[row][col]);

        if (lookup != objects.end()) {
          label = lookup->second;

          m00[label - 1]++;
          m10[label - 1] += row;
          m01[label - 1] += col;
        }

        associate_img.at<Vec3b>(Point(col, row)) = colorPalette[label];
      }
    }

    // Calculate the centroid of each object
    for (int i = 0; i < objects.size(); i++) {
      Point2f centroid((float)m01[i] / m00[i], (float)m10[i] / m00[i]);
      mc.push_back(centroid);

      // Report the centroid
      #ifdef DEBUG
      int c = centroid.x;
      int r = centroid.y;
      Point2f world = pixel2world(c, r);
      ROS_INFO_STREAM("object " << i << " centroid:  (" << c << ", " << r << ") => world(" << world.x << ", " << world.y << ")");
      #endif

      // Draw crosshairs
      line(associate_img, Point2f(centroid.x - 5, centroid.y), Point2f(
        centroid.x + 5, centroid.y), CV_RGB(0, 0, 0));
      line(associate_img, Point2f(centroid.x, centroid.y - 5), Point2f(
        centroid.x, centroid.y + 5), CV_RGB(0, 0, 0));
    }

    // Fourth raster scan
    for (int row = 0; row < height; row++) {
      for (int col = 0; col < width; col++) {
        std::map<int, int>::iterator lookup = objects.find(pixelLabel[row][col]);

        if (lookup != objects.end()) {
          int i = lookup->second - 1;

          c11[i] += (-mc[i].y + row) * (-mc[i].x + col);
          c20[i] += pow(-mc[i].y + row, 2);
          c02[i] += pow(-mc[i].x + col, 2);
        }
      }
    }

    // Calculate the orientation of each object
    for (int i = 0; i < objects.size(); i++) {
      mo.push_back(atan2(c11[i] * 2, c20[i] - c02[i]) / 2);
    }

    delete[] m00;
    delete[] m10;
    delete[] m01;
    delete[] c11;
    delete[] c20;
    delete[] c02;
  } else {
    for (int row = 0; row < height; row++) {
      for (int col = 0; col < width; col++) {
        associate_img.at<Vec3b>(Point(col, row)) = colorPalette[0];
      }
    }
  }

  // Clean up memory
  delete[] labels;
  delete[] equiv;
  for (int row = 0; row < height; row++) {
    delete[] pixelLabel[row];
  }
  delete[] pixelLabel;
  delete[] pixelCount;

  return associate_img;
}

/****************************************************
 * Function for Lab 6
 ****************************************************/
// This is a call back function of mouse click, it will be called when there's
// a click on the video window.
// You will write your coordinate transformation in this function.
void onMouse(int event, int x, int y, int flags, void* userdata) {
  if (event == EVENT_LBUTTONDOWN && !mc.empty() && !holding_block) {
    // If the robot is not holding a block, pick up the nearest one to the
    // designated row and column.
    ROS_INFO_STREAM("left click:  (" << x << ", " << y << ")");

    // Find the nearest block centroid
    size_t block = 0;
    for (size_t i = 0; i < mc.size(); i++) {
      float distance = pow(mc[i].x - x, 2) + pow(mc[i].y - y, 2);
      float closest  = pow(mc[block].x - x, 2) + pow(mc[block].y - y, 2);

      if (distance < closest) {
        block = i;
      }
    }

    Point2f world = pixel2world(mc[block].x, mc[block].y);

    ROS_INFO_STREAM("nearest block in world frame:  (" << world.x << ", " << world.y << ")");

    // Move to above block
    lab4::command msg;
    msg.x        = world.x;
    msg.y        = world.y;
    msg.z        = BLOCK_HEIGHT + 2;
    msg.pitch    = 0;
    msg.roll     = mo[block];
    msg.grip     = 0;
    msg.softhome = 0;
    pub_command_ptr->publish(msg);

    // Move down to block
    msg.z = BLOCK_HEIGHT;
    pub_command_ptr->publish(msg);

    // Grip block and move to soft home
    msg.x    = -TX;
    msg.y    = -TY;
    msg.z    = 25;
    msg.grip = 1;
    pub_command_ptr->publish(msg);

    holding_block = true;
  } else if (event == EVENT_RBUTTONDOWN && holding_block) {
    // If the robot is holding a block, place it at the designated row and
    // column.
    ROS_INFO_STREAM("right click:  (" << x << ", " << y << ")");

    Point2f world = pixel2world(x, y);

    ROS_INFO_STREAM("point in world frame:  (" << world.x << ", " << world.y << ")");

    // Move block to position
    lab4::command msg;
    msg.x        = world.x;
    msg.y        = world.y;
    msg.z        = BLOCK_HEIGHT;
    msg.pitch    = 0;
    msg.roll     = 0;
    msg.grip     = 1;
    msg.softhome = 0;
    pub_command_ptr->publish(msg);

    // Release block and move up
    msg.z    = BLOCK_HEIGHT + 2;
    msg.grip = 0;
    pub_command_ptr->publish(msg);

    // Move to soft home
    msg.x = -TX;
    msg.y = -TY;
    msg.z = 25;
    pub_command_ptr->publish(msg);

    holding_block = false;
  }
}

int main(int argc, char** argv) {
  // Initialization of ROS required for each Node
  ros::init(argc, argv, "image_converter");
  // Handler for this node
  ros::NodeHandle nh_command;
  // Define a publisher to publish to topic "rhino/command" with msg type
  // <rhino_ros::command>
  ros::Publisher pub_command = nh_command.advertise<lab4::command>(
    "/rhino/target_position", 1000);
  pub_command_ptr = &pub_command;

  // Initialize video window names
  namedWindow("Image window");
  namedWindow("gray_scale");
  namedWindow("black and white");
  namedWindow("associate objects");

  // Instantiate the class
  ImageConverter ic;

  // Initialize the mouse callback
  setMouseCallback("Image window", onMouse);
  setMouseCallback("gray_scale", onMouse);
  setMouseCallback("black and white", onMouse);
  setMouseCallback("associate objects", onMouse);

  while (ros::ok()) {
    ros::spinOnce();
  }

  return 0;
}