Subversion Repositories svnkaklik

/*
    AVRcamVIEW: A PC application to test out the functionallity of the
     AVRcam real-time image processing engine.
    Copyright (C) 2004    Brent A. Taylor

    This program is free software; you can redistribute it and/or
    modify it under the terms of the GNU General Public
    License as published by the Free Software Foundation; either
    version 2 of the License, or (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    General Public License for more details.

    You should have received a copy of the GNU General Public
    License along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

   For more information on the AVRcamVIEW, please contact:

   taylorba@comcast.net

   or go to www.jrobot.net for more details regarding the system.
*/

package avr.device;

import java.io.*;
import java.nio.*;
import java.util.*;
import javax.comm.*;

import java.awt.*;

import avr.connection.*;
import avr.connection.event.*;
import avr.device.event.*;
import avr.io.*;
import avr.lang.*;

public class Device implements ConnectionListener {

   public static void main(String[] args) throws Exception {
      Device device = new Device();

      SerialParams params = new SerialParams();

      device.setConnection(new SerialConnection("COM1", params));

      device.connect();
      device.sendPing();
      device.sendDumpFrame();
      device.sendGetVersion();
      device.sendSetRegisters(new HashMap());
      device.sendReset();
      device.sendEnableTracking();
      device.sendDisableTracking();
      device.sendSetColorMap(null, null, null);

      try {
         System.in.read();
      } catch(IOException ioe) {
         ioe.printStackTrace();
      }
      device.disconnect();

   }

   private static final String ACK = "ACK";
   private static final String NCK = "NCK";
   private static final String VERSION = "AVR";

   private static final String COLOR_MAP_KEY = "avr.color.map";

   // timeout after 3 seconds
   private static final int RESPONSE_TIMEOUT = 3000;

   private java.util.List connectionListeners;
   private java.util.List dataListeners;
   private AbstractConnection con;

   private InputStream in;
   private OutputStream out;

   private int[][] colorMap;
   private Color[] mapColors;

   private Timer responseTimer;
   private TimerTask responseTask;

   public Device() {
      connectionListeners = new ArrayList(3);
      dataListeners = new ArrayList(3);
      loadMap();

      responseTimer = new Timer();
   }

   public Color[] getMapColors() {
      return mapColors;
   }

   public int[][] getColorMap() {
      return colorMap;
   }

   public void setColorMap(int[][] colorMap) {
      this.colorMap = colorMap;
      setMapColors();
      saveMap();
   }

   public void saveMap() {
      try {
         ByteArrayOutputStream bOut = new ByteArrayOutputStream();
         DataOutputStream dOut = new DataOutputStream(bOut);

         dOut.writeInt(colorMap.length);
         for(int i = 0; i < colorMap.length; i++) {
            dOut.writeInt(colorMap[i].length);
            for(int j = 0; j < colorMap[i].length; j++) {
               dOut.writeInt(colorMap[i][j]);
            }
         }

         dOut.close();

         AVRSystem.PREFS.putByteArray(COLOR_MAP_KEY, bOut.toByteArray());

      } catch(Exception e) {
         e.printStackTrace();
      }
   }

   public void loadMap() {
      try {
         byte[] data = AVRSystem.PREFS.getByteArray(COLOR_MAP_KEY, null);
         if(data != null) {
            ByteArrayInputStream bIn = new ByteArrayInputStream(data);
            DataInputStream dIn = new DataInputStream(bIn);

            int width = dIn.readInt();
            colorMap = new int[width][];
            for(int i = 0; i < colorMap.length; i++) {
               colorMap[i] = new int[dIn.readInt()];
               for(int j = 0; j < colorMap[i].length; j++) {
                  colorMap[i][j] = dIn.readInt();
               }
            }

            dIn.close();
         } else {
            colorMap = new int[3][AVRSystem.NUM_INTENSITIES];
         }

         setMapColors();

      } catch(Exception e) {
         e.printStackTrace();
      }

   }

   private void setMapColors() {
      mapColors = new Color[8];

      for(int col = 0; col < mapColors.length; col++) {
         int value = 0;

         int red = 0;
         int green = 0;
         int blue = 0;

         int numRed = 0;
         int numGreen = 0;
         int numBlue = 0;


         for(int i = 0; i < AVRSystem.NUM_INTENSITIES; i++) {

            if((colorMap[0][i] & (0x01 << (7 - col))) != 0) {
               red += i << 4;
               numRed++;
            }

            if((colorMap[1][i] & (0x01 << (7 - col))) != 0) {
               green += i << 4;
               numGreen++;
            }

            if((colorMap[2][i] & (0x01 << (7 - col))) != 0) {
               blue += i << 4;
               numBlue++;
            }

         }

         if(numRed > 0) {
            red /= numRed;
         }

         if(numGreen > 0) {
            green /= numGreen;
         }

         if(numBlue > 0) {
            blue /= numBlue;
         }

         value = (red << 16) | (green << 8) | blue;

         mapColors[col] = new Color(value);
      }

   }

   public AbstractConnection getConnection() {
      return con;
   }

   public void setConnection(AbstractConnection con) {
      this.con = con;
   }

   public boolean isConnected() {
      return con != null && con.isConnected();
   }

   public void addConnectionListener(ConnectionListener listener) {
      AVRSystem.LOG.finest("Added Connection Listener: " + listener);
      connectionListeners.add(listener);
   }

   public void removeConnectionListener(ConnectionListener listener) {
      AVRSystem.LOG.finest("Removed Connection Listener: " + listener);
      connectionListeners.remove(listener);
   }

   public void connected(ConnectionEvent event) {
      ConnectionListener[] listeners = (ConnectionListener[])connectionListeners.toArray(new ConnectionListener[connectionListeners.size()]);
      for(int i = 0; i < listeners.length; i++) {
         ((ConnectionListener)listeners[i]).connected(event);
      }
   }

   public void disconnected(ConnectionEvent event) {
      ConnectionListener[] listeners = (ConnectionListener[])connectionListeners.toArray(new ConnectionListener[connectionListeners.size()]);
      for(int i = 0; i < listeners.length; i++) {
         ((ConnectionListener)listeners[i]).disconnected(event);
      }
   }

   public void connect() throws Exception {

      if(!isConnected()) {
         con.connect();

         if(con instanceof SerialConnection) {
            SerialPort serialPort = (SerialPort)con.getConnectionObject();
            serialPort.notifyOnDataAvailable(true);
            try {
               serialPort.addEventListener(new SerialEventHandler(this));
            } catch(TooManyListenersException tmle) {
               AVRSystem.LOG.severe(tmle.getMessage());
            }
         }

         in = new AVRInputStream(con.getInputStream());
         out = con.getOutputStream();

         AVRSystem.LOG.config("Device connected to " + con.toString());

         connected(new ConnectionEvent(con));

      }
   }

   public void disconnect() {

      if(isConnected()) {
         try {
            con.disconnect();
            AVRSystem.LOG.config("Device Disconnected");
            disconnected(new ConnectionEvent(con));
            con = null;
         } catch(IOException ioe) {
            AVRSystem.LOG.severe(ioe.getMessage());
         }
      }
   }

   public InputStream getInputStream() {
      return in;
   }

   public void addDataListener(DataListener dl) {
      if(dataListeners.add(dl)) {
         AVRSystem.LOG.finest("Added Data Listener: " + dl);
      }
   }

   public void removeDataListener(DataListener dl) {
      if(dataListeners.remove(dl)) {
         AVRSystem.LOG.finest("Removed Data Listener: " + dl);
      }
   }

   protected void handleString(String data) {
      if(data.equals(ACK)) {
         fireACKReceived();
      } else if(data.equals(NCK)) {
         fireNCKReceived();
      } else if(data.startsWith(VERSION)) {
         fireVERSIONReceived(data);
      } else {
         StringBuffer builder = new StringBuffer("UNKNOWN PACKET: (");
         ByteBuffer bytes = ByteBuffer.wrap(data.getBytes());
         builder.append(bytes.remaining()).append(") ");
         while(bytes.hasRemaining()) {
            builder.append(Integer.toHexString(bytes.get() & 0xFF)).append(' ');
         }
         AVRSystem.LOG.warning(builder.toString());
      }
   }

   protected void handleData(ByteBuffer data) {
      if(data.hasRemaining()) {
         byte dataType = data.get();
         if(dataType == 0x0B) {
            fireFrameDataReceived(data);
         } else if(dataType == 0x0A) {
            fireTrackingDataReceived(data);
         } else {
            handleString(new String(data.array(), 0, data.limit()));
         }
      }
   }

   protected void fireACKReceived() {
      if(responseTask != null) {
         responseTask.cancel();
      }
      AVRSystem.LOG.info("Received: ACK");
      DataListener[] listeners = (DataListener[])dataListeners.toArray(new DataListener[dataListeners.size()]);
      for(int i = 0; i < listeners.length; i++) {
         ((DataListener)listeners[i]).ack();
      }
   }

   private void fireResponseTimerExpired() {
      AVRSystem.LOG.severe("Response Timer Expired");
      DataListener[] listeners = (DataListener[])dataListeners.toArray(new DataListener[dataListeners.size()]);
      for(int i = 0; i < listeners.length; i++) {
         ((DataListener)listeners[i]).responseTimerExpired();
      }
   }

   protected void fireNCKReceived() {
      if(responseTask != null) {
         responseTask.cancel();
      }
      AVRSystem.LOG.info("Received: NCK");
      DataListener[] listeners = (DataListener[])dataListeners.toArray(new DataListener[dataListeners.size()]);
      for(int i = 0; i < listeners.length; i++) {
         ((DataListener)listeners[i]).nck();
      }
   }

   protected void fireVERSIONReceived(String version) {
      if(responseTask != null) {
         responseTask.cancel();
      }
      AVRSystem.LOG.info("Received: " + version);
      DataListener[] listeners = (DataListener[])dataListeners.toArray(new DataListener[dataListeners.size()]);
      for(int i = 0; i < listeners.length; i++) {
         ((DataListener)listeners[i]).version(version);
      }
   }

   protected void fireFrameDataReceived(ByteBuffer data) {

      int position = data.position();
      StringBuffer buf = new StringBuffer("Received: Frame Dump (" + (data.get(position) & 0xFF) + ")");

      data.position(0);

      while(data.hasRemaining()) {
         int b = data.get() & 0xFF;
         buf.append(' ').append(((b & 0xF0) == 0) ? "0" : "").append(Integer.toHexString(b).toUpperCase());
      }

      data.position(position);

      AVRSystem.LOG.info(buf.toString());
//      AVRSystem.LOG.info("Received: Frame Dump (" + (data.get(data.position()) & 0xFF) + ")");
      DataListener[] listeners = (DataListener[])dataListeners.toArray(new DataListener[dataListeners.size()]);
      for(int i = 0; i < listeners.length; i++) {
         data.mark();
         ((DataListener)listeners[i]).frameData(data);
         data.reset();
      }
   }

   protected void fireTrackingDataReceived(ByteBuffer data) {

      int position = data.position();
      StringBuffer buf = new StringBuffer("Received: Tracking Info (" + (data.get(position) & 0xFF) + ")");

      data.position(0);

      while(data.hasRemaining()) {
         int b = data.get() & 0xFF;
         buf.append(' ').append(((b & 0xF0) == 0) ? "0" : "").append(Integer.toHexString(b).toUpperCase());
      }

      data.position(position);

      AVRSystem.LOG.info(buf.toString());
      DataListener[] listeners = (DataListener[])dataListeners.toArray(new DataListener[dataListeners.size()]);
      for(int i = 0; i < listeners.length; i++) {
         data.mark();
         ((DataListener)listeners[i]).trackingData(data);
         data.reset();
      }
   }

   private void sendRequest(byte[] data) throws IOException {
      sendRequest(data, 0, data.length);
   }

   private void sendRequest(byte[] data, int off, int len) throws IOException {

      responseTask = new ResponseTask();
      responseTimer.schedule(responseTask, RESPONSE_TIMEOUT);

      if(isConnected()) {

         out.write(data, off, len);
         out.write((byte)'\n');
         out.flush();

         StringBuffer builder = new StringBuffer("Sending: ");
         for(int i = off; i < len; i++) {
            builder.append((char)data[i]);
         }
         AVRSystem.LOG.info(builder.toString());
      } else {
         AVRSystem.LOG.warning("AVRcam not connected.");
      }
   }

   public void sendSetRegisters(Map registers) throws IOException {
      ByteBuffer buffer = ByteBuffer.allocate(60);
      buffer.put((byte)'C')
            .put((byte)'R');

     Set entries = registers.entrySet();
     for(Iterator i = entries.iterator(); i.hasNext();) {
        Map.Entry entry = (Map.Entry)i.next();
        buffer.put((byte)' ').put(entry.getKey().toString().getBytes())
              .put((byte)' ').put(entry.getValue().toString().getBytes());
     }

      buffer.put((byte)'\r');
      buffer.flip();
      sendRequest(buffer.array(), 0, buffer.remaining());
   }

   public void sendDisableTracking() throws IOException {
      sendRequest(new byte[] { (byte)'D', (byte)'T', (byte)'\r'} );
   }

   public void sendDumpFrame() throws IOException {
      sendRequest(new byte[] { (byte)'D', (byte)'F', (byte)'\r'} );
   }

   public void sendEnableTracking() throws IOException {
      sendRequest(new byte[] { (byte)'E', (byte)'T', (byte)'\r'} );
   }

   public void sendGetVersion() throws IOException {
      sendRequest(new byte[] { (byte)'G', (byte)'V', (byte)'\r'} );
   }

   public void sendPing() throws IOException {
      sendRequest(new byte[] { (byte)'P', (byte)'G', (byte)'\r'} );
   }

   public void sendReset() throws IOException {
      sendRequest(new byte[] { (byte)'R', (byte)'S', (byte)'\r'} );
   }

   public void sendSetColorMap(int[] red, int[] green, int[] blue) throws IOException {
      ByteBuffer buffer = ByteBuffer.allocate(2 + 4 * red.length * 3 + 1);
      buffer.put("SM".getBytes());
      for(int i = 0; i < red.length; i++) {
         buffer.put((" " + red[i] + "").getBytes());
      }
      for(int i = 0; i < green.length; i++) {
         buffer.put((" " + green[i] + "").getBytes());
      }
      for(int i = 0; i < blue.length; i++) {
         buffer.put((" " + blue[i] + "").getBytes());
      }
      buffer.put("\r".getBytes());
      buffer.flip();
      sendRequest(buffer.array(), 0, buffer.remaining());
   }

   public void sendCameraData(ByteBuffer data) throws IOException {

      out.write((byte)0x0A);
      out.write(data.array(), 0, data.remaining());
      out.write((byte)0xFF);

      out.flush();

   }

   private final static class SerialEventHandler implements SerialPortEventListener {

      private Device device;

      public SerialEventHandler(Device device) {
         this.device = device;
      }

      public void serialEvent(SerialPortEvent spe) {
         if(spe.getEventType() == SerialPortEvent.DATA_AVAILABLE) {

            try {

               do {

                  byte[] data = new byte[1024];

                  int bytesRead = device.getInputStream().read(data);
                  if(bytesRead > 0) {
                     // take only bytesRead - 1 to remove the
                     // terminating character '\r' or 0xFF
                     EventQueue.invokeLater(new GUITask(device, ByteBuffer.wrap(data, 0, bytesRead - 1)));
                  }

               } while(device.getInputStream().available() > 0);

            } catch(IOException ioe) {
               AVRSystem.LOG.severe(ioe.getMessage());
               device.disconnect();
            }

         }
      }

   }

   private final static class GUITask implements Runnable {
      private ByteBuffer data;
      private Device device;

      public GUITask(Device device, ByteBuffer data) {
         this.device = device;
         this.data = data;
      }

      public void run() {
         device.handleData(data);
      }

   }

   private final class ResponseTask extends TimerTask {

      public void run() {
         // make sure we fire the timer expired event
         // using the Event Dispatch Thread.  If not,
         // queue this TimerTask in the Event Queue.
         if(EventQueue.isDispatchThread()) {
            fireResponseTimerExpired();
         } else {
            EventQueue.invokeLater(this);
         }
      }

   }

   /**
    * Only used for debugging purposes, this will simulate an ACK coming from the device.
    */
   public void simulateACK() {
      fireACKReceived();
   }

   /**
    * Only used for debugging purposes, this will simulate an NCK coming from the device.
    */
   public void simulateNCK() {
      fireNCKReceived();
   }

}