package sivantoledo.nxt;

import java.util.concurrent.locks.*;

import sivantoledo.nxt.CommunicationInterface.Listener;

public class FantomInterface implements CommunicationInterface {
  
  static { 
    java.io.FileOutputStream dll_f;
    java.io.InputStream      dll_r;
    try {
      System.loadLibrary("FantomInterface"); 
    } catch (UnsatisfiedLinkError ule) {
      try {
        dll_r = FantomInterface.class.getClassLoader().getResourceAsStream("native/win32/Fantom/release/FantomInterface.dll"); 
        dll_f = new java.io.FileOutputStream("FantomInterface.dll");
        byte[] buffer = new byte[8192];
        int    bytes_read = 0;
        do {
          dll_f.write(buffer, 0, bytes_read);
          bytes_read = dll_r.read(buffer);
        } while (bytes_read != -1);
        dll_r.close();
        dll_f.close();
        System.loadLibrary("FantomInterface");
      } catch (java.io.IOException ioe) {
        System.out.println("Error encountered while trying to copy a DLL to the working directory: "
                           +ioe.getMessage());
      }
    }
  }

  /* 
   * The JNI DLL is not thread safe, so we use a lock.
   */
  
  private static Lock fantom_lock = new ReentrantLock();

  public native static String scanFirst();
  public native static String scanNext();

  public native static int    connect(String resource_name);
  public native static void   disconnect();

  public native static int    rawsend(byte[] buffer, int length);
  public native static int    rawreceive(byte[] buffer);
  
  public native static int    directCommand(byte[] cmd, int cmd_length,
                                            byte[] rsp, int rsp_length);

  public static void main(String[] arguments) {
    //java.util.Map<String,String> map = enumerateInterfaces();
    //if (true) return;
    String name;
    
    //name = "NXT";    
    //System.out.println("connect to "+name+" returned "+connect(name));
    
    name = "BTH::NXToledo::00:16:53:01:94:67::15";    
    name = "USB0::0x0694::0x0002::001653019467::RAW";    
    int error = connect(name);
    System.out.printf("connect to %s returned %d\n",name,error);
    if (error != 0) return;
    
    byte[] buffer = { (byte) 0x80, 0x03, 0x00, 0x08, (byte) 0xff, 0x00 };
    
    rawsend(buffer,buffer.length);
    
    byte[] directCmd = { 0x13, 0x0a, 0x00, 0x01 };
    byte[] directRsp = new byte[256];
    byte[] directCmdMW = { 0x09, 0x00, 0x05, 
        'X', 'Y', 'Z', '8', 0x00 };
    
    long t1 = System.currentTimeMillis();
    for (int i=0; i<10; i++) {
      int bytes_read = directCommand(directCmd,4,directRsp,63);
      //int bytes_read = directCommand(directCmdMW,8,directRsp,1);
    
      System.out.println("Bytes read = "+bytes_read);
      for (int b=0; b<bytes_read; b++) 
        System.out.printf("%02x ",directRsp[b]);
      System.out.println("");
    }
    long t2 = System.currentTimeMillis();
    System.out.println("time (milliseconds) = "+(t2-t1));
    
    for (int i=0; i<00; i++) {
      //byte[] message_read = { 0x00, 0x13, 0x0a, 0x00, 0x01 };
      byte[] message_read = { 0x01, (byte) 0x9b };
      byte[] message_write = { (byte) 0x80, 0x09, 0x00, 0x05, 
                                'X', 'Y', 'Z', '8', 0x00 };
      //int sent = 0;
      //while (sent != message_write.length)
      //  sent = rawsend(message_write,message_write.length);
      //try { Thread.sleep(7); } catch (Exception e) {}
      
      rawsend(message_read,message_read.length);
      System.out.printf("receive returned %d\n",rawreceive(buffer));
      System.out.printf("receive returned %d\n",rawreceive(buffer));
      System.out.printf("receive returned %d\n",rawreceive(buffer));
      System.out.printf("receive returned %d\n",rawreceive(buffer));
      System.out.printf("receive returned %d\n",rawreceive(buffer));
      System.out.printf("receive returned %d\n",rawreceive(buffer));
      System.out.printf("receive returned %d\n",rawreceive(buffer));
      System.out.printf("receive returned %d\n",rawreceive(buffer));
      System.out.printf("receive returned %d\n",rawreceive(buffer));
      System.out.printf("receive returned %d\n",rawreceive(buffer));
    }
    
    rawsend(buffer,buffer.length);
    
    disconnect();
    

    //name = "BTH::NXToledo::00:16:53:01:94:67::15";
    
    //System.out.println("connect to "+name+" returned "+connect(name));
  }  
  
  public static java.util.Map<String,String> enumerateInterfaces() {
    java.util.Map<String,String> map = new java.util.LinkedHashMap<String,String>();

    String resource_name;
    
    fantom_lock.lock();

    resource_name = scanFirst();
    while (resource_name != null) {
      //System.out.println(resource_name);

      //if (resource_name.startsWith("USB")) {
      //  map.put(resource_name, "USB");
      //  Log.println(resource_name + " ~ " + "USB");
      //}
      
      if (resource_name.startsWith("BTH")) {
        int semi1, semi2;
        semi1 = resource_name.indexOf("::");
        semi1 ++;
        semi1 ++;
        semi2 = resource_name.indexOf("::",semi1);
        
        String external_name = resource_name.substring(semi1, semi2);
        
        map.put(resource_name, external_name);
        Log.println(resource_name + " ~ " + external_name);
      }
      resource_name = scanNext();
    }
    fantom_lock.unlock();
    
    return map;
  }
  
  /*
   * Constructor
   */
  
  private boolean connected = false;
  private String  resource_name;
  
  public FantomInterface(String resource_name, String short_name, Listener l) 
    throws java.io.IOException {
    
    this.resource_name = resource_name;
    //name = "BTH::NXToledo::00:16:53:01:94:67::15";
    fantom_lock.lock();
    int error = connect(resource_name);
    System.out.printf("connect to %s returned error code %d\n",resource_name,error);
    if (error != 0) {
      System.out.printf("trying again...\n");      
      error = connect(resource_name);
      System.out.printf("connect to %s returned error code %d\n",resource_name,error);
    }
    fantom_lock.unlock();
    if (error == 0) 
      connected = true;
    else {
      connected = false; 
      return;
    }
    
    for (int q=0; q<10; q++)
      sendQueue[q] = new java.util.LinkedList<Message>();

    messageListener = l;
    commThread = new CommThread();    
    commThread.start();
  }

  private java.util.Queue<Message>[] sendQueue = new java.util.Queue[10];
  private Lock      lock = new ReentrantLock();
  //private Condition notEmpty = lock.newCondition(); 
  
  public void flushAllQueues() {
    if (!connected) return;
    lock.lock();
    for (int q=0; q<sendQueue.length; q++) sendQueue[q].clear();
    lock.unlock();
  }

  public void flushQueue(int q) {
    if (!connected) return;
    lock.lock();
    sendQueue[q].clear();
    lock.unlock();
  }

  public void send(Message m) {
    if (!connected) return;
    lock.lock();
    Log.println(">> GOING TO ENQUEUE A SEND ON QUEUE "+m.mailbox+" QUEUE SIZE "+sendQueue[m.mailbox].size());
    try {
      //Log.println(">> ENQUEUEING SEND to queue "+buffer[2]);
      sendQueue[m.mailbox].add(m);
      //notEmpty.signal();
    } finally {
      lock.unlock();
    }
    //Log.println(">> DONE ENQUEUEING SEND");
  }
  
  private CommThread commThread;
  private class CommThread extends Thread {
    private volatile boolean stopSignal = false;
    public void done() { stopSignal = true; }
    public void run() {
      try {
      fantom_lock.lock();
      byte[] response = new byte[256];
      byte[] msg_read = { 0x13, 0x0a, 0x00, 0x01 };
      int bytes_read;
      while (!stopSignal) {
        boolean sent_message     = false;
        boolean received_message = false;
        boolean sent_now = false;
        do {
          //System.out.println("sending");
          sent_now = false;
          for (int q=0; q<10; q++) {
            Message m = null;
            lock.lock();
            if ( !sendQueue[q].isEmpty() ) {
              m = sendQueue[q].poll();
              // drop old unimportant messages
              while (sendQueue[q].size() >= 5 && m.important == false)
                m = sendQueue[q].poll();
            }
            lock.unlock();
            if (m != null) {
              sent_message = true;
              sent_now = true;
              byte[] command = new byte[ m.payload.length + 2 ];
              command[0] = 0x09; // message write
              command[1] = m.mailbox;
              for (int i=0; i<m.payload.length; i++)
                command[2+i] = m.payload[i];
              if (m.important) {
                System.out.println("sending an important message");
                bytes_read = directCommand(command,command.length,response,2);
              } else
                bytes_read = directCommand(command,command.length,null,0);
              //System.out.printf("message sent, %d bytes read: ",bytes_read);
              //for (int i=0; i<bytes_read; i++) System.out.printf("%02x ",response[i]);
              //System.out.println();
            }
          } // loop over mailboxes
        } while (sent_now == true);

        // query the other side for messages
        
        do {
          //System.out.println("receiving");
          received_message = false;
          bytes_read = directCommand(msg_read,4,response,63);
          
          if (bytes_read < 0) {
            System.out.println("receive error "+bytes_read);
            bytes_read = directCommand(msg_read,4,response,63);
            if (bytes_read < 0) {
              disconnect();
              int error = connect(resource_name);
              System.out.printf("connect to %s returned error code %d\n",resource_name,error);
              if (error != 0) {
                System.out.printf("trying again...\n");      
                error = connect(resource_name);
                System.out.printf("connect to %s returned error code %d\n",resource_name,error);
              }
              if (error == 0) {
                bytes_read = directCommand(msg_read,4,response,63);
                if (bytes_read < 0)
                  System.out.println("receive error "+bytes_read+", all efforts to reconnect failed");
              }
            }
          }
          if (bytes_read > 1) {
            if (response[1] == (byte) 0xEC) {
              //System.out.println("no program running on the NXT");
              flushAllQueues();
              //try { Thread.sleep(500); } catch (Exception e) {}
            }
            if (response[1] == (byte) 0x40) {
              //System.out.println("empty mailbox");
              //if (!sent_message)
              //  try { Thread.sleep(50); } catch (Exception e) {}
            }
            if (response[1] == (byte) 0x00) {              
              received_message = true;
              messageListener.process(response[2], response, 3);
              //System.out.printf("message received, %d bytes read: ",bytes_read);
              //for (int i=0; i<bytes_read; i++) System.out.printf("%02x ",response[i]);
              //System.out.println();
            }
          }
        } while (false); // bytes_read > 0 && response[1] == 0);
        
        //if (!sent_message && !received_message)
        //  try { Thread.sleep(50); } catch (Exception e) {}
      } 
    } finally {      
      System.out.println("closing connection from the finally clause");
      disconnect();
      fantom_lock.unlock();
      connected = false;
      Log.println("close done");  
    }
  }
  }

  private Listener messageListener;

  public void close() {
    if (!connected) return;
    Log.println("calling done on threads");
    //sendThread.done();
    if (commThread != null) commThread.done();
    try {
      Log.println("calling join()");
      if (commThread != null) commThread.interrupt();
      if (commThread != null) commThread.join();
    } catch ( InterruptedException ex ) {}
    //Log.println("now closing the connection");
    //fantom_lock.lock();
    //disconnect();
    //fantom_lock.unlock();
  }

}