IomDriver.java

/*
 * Copyright (C) 2013 Google Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */

package interactivespaces.hardware.driver.gaming.wilddivine;

import interactivespaces.InteractiveSpacesException;
import interactivespaces.hardware.driver.DriverSupport;
import interactivespaces.service.comm.usb.UsbCommunicationEndpoint;
import interactivespaces.service.comm.usb.UsbCommunicationEndpointService;
import interactivespaces.service.comm.usb.internal.libusb4j.Usb4JavaUsbCommunicationEndpointService;
import interactivespaces.system.StandaloneInteractiveSpacesEnvironment;
import interactivespaces.util.InteractiveSpacesUtilities;
import interactivespaces.util.concurrency.CancellableLoop;

import com.google.common.collect.Lists;

import java.util.List;

/**
 * A driver for the Wild Divine Iom biofeedback sensor.
 *
 * @author Keith M. Hughes
 */
public class IomDriver extends DriverSupport {

  public static void main(String[] args) {
    final StandaloneInteractiveSpacesEnvironment spaceEnvironment = StandaloneInteractiveSpacesEnvironment.newStandaloneInteractiveSpacesEnvironment();
    Usb4JavaUsbCommunicationEndpointService service = new Usb4JavaUsbCommunicationEndpointService();
    spaceEnvironment.getServiceRegistry().registerService(service);
    service.startup();

    IomDriver driver = new IomDriver();
    driver.prepare(spaceEnvironment, spaceEnvironment.getSystemConfiguration(), spaceEnvironment.getLog());

    driver.addListener(new IomDeviceListener() {

      @Override
      public void onIomMeasurement(IomDriver driver, double heartRateValue, double skinConductivityLevel) {
        spaceEnvironment.getLog().info(String.format("Heart is %f, skin is %f", heartRateValue, skinConductivityLevel));
      }
    });

    try {
      driver.startup();

      InteractiveSpacesUtilities.delay(20000);
    } catch (Exception e) {
      e.printStackTrace();
    } finally {
      driver.shutdown();
      spaceEnvironment.shutdown();
    }
  }

  /**
   * Character for the beginning of a tag.
   */
  public static final byte CHARACTER_TAG_BEGIN = 0x3c;

  /**
   * Character for the end of a tag.
   */
  public static final byte CHARACTER_TAG_END = 0x3e;

  /**
   * Character for the the slash part of a tag.
   */
  public static final byte CHARACTER_SLASH_TAG = 0x5c;

  /**
   * Character for the RAW message tag.
   */
  public static final byte CHARACTER_RAW_START = 0x52;

  /**
   * Hex digit for A.
   */
  public static final byte HEX_DIGIT_A = 'A';

  /**
   * Hex digit for F.
   */
  public static final byte HEX_DIGIT_F = 'F';

  /**
   * Hex digit for 0.
   */
  public static final byte HEX_DIGIT_0 = '0';

  /**
   * Hex digit for 9.
   */
  public static final byte HEX_DIGIT_9 = '9';

  /**
   * Communication endpoint for talking to sensor.
   */
  private UsbCommunicationEndpoint endpoint;

  /**
   * The buffer for reading content into.
   *
   * <p>
   * The size of this buffer is the report size for the device.
   */
  private byte[] readBuffer;

  /**
   * The size of the read buffer, in bytes.
   */
  private int readBufferSize;

  /**
   * The current position in the read buffer.
   */
  private int curReadPosition;

  /**
   * How many bytes in the current packet are good?
   */
  private int curPacketSize;

  /**
   * The listeners for this driver.
   */
  private List<IomDeviceListener> listeners = Lists.newArrayList();

  /**
   * The sensor read loop.
   */
  private CancellableLoop sensorReadLoop;

  @Override
  public void startup() {
    UsbCommunicationEndpointService service =
        spaceEnvironment.getServiceRegistry().getRequiredService(UsbCommunicationEndpointService.SERVICE_NAME);

    endpoint = service.newEndpoint("14fa", "0001");

    if (endpoint == null) {
      throw new InteractiveSpacesException("Cannot locate an Iom device on the USB bus");
    }

    // The IOM reader is second endpoint for this port.
    endpoint.setEndpointIndex(1);

    endpoint.startup();

    readBuffer = endpoint.newBuffer();
    readBufferSize = endpoint.getReadBufferSize();

    sensorReadLoop = new CancellableLoop() {

      @Override
      protected void loop() throws InterruptedException {
        readSensorMessage();
      }
    };

    // Having no idea where the message stream might be, just read
    // until the end of the current message in progress.
    readUntilEndOfMessage();

    spaceEnvironment.getExecutorService().execute(sensorReadLoop);
  }

  @Override
  public void shutdown() {
    if (endpoint != null) {
      sensorReadLoop.cancel();

      endpoint.shutdown();
    }
  }

  /**
   * Add a new listener to the driver.
   *
   * @param listener
   *          the listener to add
   */
  public void addListener(IomDeviceListener listener) {
    listeners.add(listener);
  }

  /**
   * Remove a listener from the driver.
   *
   * <p>
   * Does nothing if the listener was never added.
   *
   * @param listener
   *          the listener to remove
   */
  public void removeListener(IomDeviceListener listener) {
    listeners.remove(listener);
  }

  /**
   * Read a new message from the sensor.
   */
  private void readSensorMessage() {
    while (true) {
      byte b = readByte();
      if (b != CHARACTER_TAG_BEGIN) {
        return;
      }

      b = readByte();
      if (b == CHARACTER_RAW_START) {
        // Skip over AW>
        readByte();
        readByte();
        b = readByte();
        if (b != CHARACTER_TAG_END) {
          return;
        }

        byte a1 = readByte();
        byte a2 = readByte();
        byte b1 = readByte();
        byte b2 = readByte();
        readByte(); // Just a space, can be ignored
        byte c1 = readByte();
        byte c2 = readByte();
        byte d1 = readByte();
        byte d2 = readByte();
        b = readByte();
        if (b != CHARACTER_TAG_BEGIN) {
          return;
        }

        // Just consume the rest
        readUntilEndOfMessage();

        // Now have all the message bits. Reconstruct the message.
        int a1v = a1 - ((a1 > HEX_DIGIT_9) ? (HEX_DIGIT_A - 10) : HEX_DIGIT_0);
        int a2v = a2 - ((a2 > HEX_DIGIT_9) ? (HEX_DIGIT_A - 10) : HEX_DIGIT_0);
        int b1v = b1 - ((b1 > HEX_DIGIT_9) ? (HEX_DIGIT_A - 10) : HEX_DIGIT_0);
        int b2v = b2 - ((b2 > HEX_DIGIT_9) ? (HEX_DIGIT_A - 10) : HEX_DIGIT_0);
        int c1v = c1 - ((c1 > HEX_DIGIT_9) ? (HEX_DIGIT_A - 10) : HEX_DIGIT_0);
        int c2v = c2 - ((c2 > HEX_DIGIT_9) ? (HEX_DIGIT_A - 10) : HEX_DIGIT_0);
        int d1v = d1 - ((d1 > HEX_DIGIT_9) ? (HEX_DIGIT_A - 10) : HEX_DIGIT_0);
        int d2v = d2 - ((d2 > HEX_DIGIT_9) ? (HEX_DIGIT_A - 10) : HEX_DIGIT_0);

        double heartRateValue = (((a1v << 8 | a2v) << 8) | (b1v << 8 | b2v)) * 0.01;
        double skinConductivityLevel = (((c1v << 8 | c2v) << 8) | (d1v << 8 | d2v)) * 0.001;

        signalListeners(heartRateValue, skinConductivityLevel);

        break;
      } else {
        // Ignore any other messages
        readUntilEndOfMessage();
      }
    }
  }

  /**
   * Signal all listeners with the sensor values.
   *
   * @param heartRateValue
   *          the heart rate value
   * @param skinConductivityLevel
   *          the skin conductivity value
   */
  public void signalListeners(double heartRateValue, double skinConductivityLevel) {
    for (IomDeviceListener listener : listeners) {
      try {
        listener.onIomMeasurement(this, heartRateValue, skinConductivityLevel);
      } catch (Exception e) {
        log.error("Error while running IomDevice listener", e);
      }
    }
  }

  /**
   * Read and throw away bytes until the end of the current message.
   */
  private void readUntilEndOfMessage() {
    // Read to the end of the current message
    while (true) {
      // First look for the end of a tag
      while (readByte() != CHARACTER_TAG_END) {
        // Nothing to do
      }

      byte b = readByte();
      if (b == 0x0a) {
        b = readByte();
        if (b == 0x0d) {
          break;
        }
      }
    }
  }

  /**
   * Read a byte from the endpoint.
   *
   * @return the byte read
   */
  private byte readByte() {
    try {
      if (curReadPosition >= curPacketSize) {
        int length = endpoint.readReportSync(readBuffer);

        curPacketSize = (readBuffer[0] & 0xff) + 1;

        // Want to start at position 1 because first byte is ignored.
        curReadPosition = 1;
      }

      return readBuffer[curReadPosition++];
    } catch (Exception e) {
      throw new InteractiveSpacesException("Could not read USB device", e);
    }
  }
}