import netP5.*;
import oscP5.*;


/**
 *     DOUBLE BLACK DIAMOND        DOUBLE BLACK DIAMOND
 *
 *         //\\   //\\                 //\\   //\\  
 *        ///\\\ ///\\\               ///\\\ ///\\\
 *        \\\/// \\\///               \\\/// \\\///
 *         \\//   \\//                 \\//   \\//
 *
 *        EXPERTS ONLY!!              EXPERTS ONLY!!
 *
 * This class implements the output function to the Panda Boards. It
 * will be moved into GLucose once stabilized.
 */
public static class PandaDriver {
  
  interface Listener {
    public void onToggle(boolean enabled);
  }
  
  private Listener listener = null;
  
  // IP address
  public final String ip;

  public PandaMapping pm;
  
  private final static int PORT = 779;
  
  private final DatagramSocket socket;
  
  // Address to send to
  private final NetAddress address;
  
  // Whether board output is enabled
  private boolean enabled = false;

  // Frame count for Grizzlies
  private int frameNum = 1;
  
  // OSC message
  private final OscMessage message;

  // List of point indices that get sent to this board
  private final int[] points;
    
  // Packet data
  private final byte[] packet = new byte[4*280]; // magic number, our UDP packet size

  private static final int NO_POINT = -1;

  private Model _model;

  ////////////////////////////////////////////////////////////////
  //
  // READ THIS RIGHT NOW BEFORE YOU MODIFY THE BELOW!!!!!!!!!!!!!
  // READ THIS RIGHT NOW BEFORE YOU MODIFY THE BELOW!!!!!!!!!!!!!
  // READ THIS RIGHT NOW BEFORE YOU MODIFY THE BELOW!!!!!!!!!!!!!
  //
  // The mappings below indicate the physical order of strips
  // connected to a pandaboard channel. The strip numbers are a
  // reflection of how the model is built.
  //
  // For ANYTHING in the model which is a rectangular prism,
  // which means Cubes, the BassBox, and each Speaker, the
  // strips are numbered incrementally by face. The first
  // face is always the FRONT, which you are looking at.
  // The next face is the RIGHT, then the BACK, then the LEFT.
  //
  // For every face, the strips are ordered numerically moving
  // clockwise from the the TOP LEFT.
  //
  // So, for a cube:
  //
  //  Strip 0: front face, top strip, left to right
  //  Strip 1: front face, right strip, top to bottom
  //  Strip 2: front face, bottom strip, right to left
  //  Strip 3: front face, left strip, bottom to top
  //
  //  Strip 4: right face, top strip, left to right
  //  ... and so on
  //  Strip 14: left face, bottom strip, right to left
  //  Strip 15: left face, left strip, bottom to top
  //
  ////////////////////////////////////////////////////////////////
  
  private final static int FORWARD = -1;
  private final static int BACKWARD = -2;

  /**
   * These constant arrays indicate the order in which the strips of a cube
   * are wired. There are four different options, depending on which bottom
   * corner of the cube the data wire comes in.
   */
  private final static int[][] CUBE_STRIP_ORDERINGS = new int[][] {
//    {  2,  1,  0,  3, 13, 12, 15, 14,  4,  7,  6,  5, 11, 10,  9,  8 }, // FRONT_LEFT
//    {  6,  5,  4,  7,  1,  0,  3,  2,  8, 11, 10,  9, 15, 14, 13, 12 }, // FRONT_RIGHT
//    { 14, 13, 12, 15,  9,  8, 11, 10,  0,  3,  2,  1,  7,  6,  5,  4 }, // REAR_LEFT
//    { 10,  9,  8, 11,  5,  4,  7,  6, 12, 15, 14, 13,  3,  2,  1,  0 }, // REAR_RIGHT


    {  2,  1,  0,  3, 13, 12, 15, 14,  4,  7,  6,  5, 11, 10,  9,  8 }, // FRONT_LEFT
    {  6,  5,  4,  7,  1,  0,  3,  2,  8, 11, 10,  9, 15, 14, 13, 12 }, // FRONT_RIGHT
    { 14, 13, 12, 15,  9,  8, 11, 10,  0,  3,  2,  1,  7,  6,  5,  4 }, // REAR_LEFT
    {  9,  8, 11,  5,  4,  7,  6, 10, 14,  2,  1,  0,  3, 13, 12, 15 }, // REAR_RIGHT

  };
  
  private final static int[][] BASS_STRIP_ORDERING = {
    // front face, counterclockwise from bottom front left
    {2, BACKWARD /* if this strip has extra pixels, you can add them here */ /*, 4 */ }, 
    {1, BACKWARD /* if this strip is short some pixels, substract them here */ /*, -3 */ },
    {0, BACKWARD },
    {3, BACKWARD },
    
    // left face, counterclockwise from bottom front left
    {13, BACKWARD },
    {12, BACKWARD },
    {15, BACKWARD },
    {14, BACKWARD },

    // back face, counterclockwise from bottom rear left
    {9, BACKWARD },
    {8, BACKWARD },
    {11, BACKWARD },
    {10, BACKWARD },

    // right face, counterclockwise from bottom rear right
    {5, BACKWARD },
    {4, BACKWARD },
    {7, BACKWARD },
    {6, BACKWARD },
  };

  private final static int[][] STRUT_STRIP_ORDERING = {
    {6, BACKWARD},
    {5, FORWARD},
    {4, BACKWARD},
    {3, FORWARD},
    {2, BACKWARD},
    {1, FORWARD},
    {0, BACKWARD},
    {7, FORWARD},    
  };
  
  private final static int[][] FLOOR_STRIP_ORDERING = {
    {0, FORWARD},
    {1, FORWARD},
    {2, FORWARD},
    {3, BACKWARD},
  };
  
  // The speakers are currently configured to be wired the same
  // as cubes with Wiring.FRONT_LEFT. If this needs to be changed,
  // remove this null assignment and change the below to have mappings
  // for the LEFT and RIGHT speaker
  private final static int[][][] SPEAKER_STRIP_ORDERING = {
    // Left speaker
    { 
      // Front face, counter-clockwise from bottom left
      {2, BACKWARD },
      {1, BACKWARD },
      {0, BACKWARD },
      {3, BACKWARD },
    },
    // Right speaker
    {
      // Front face, counter-clockwise from bottom left
      {2, BACKWARD },
      {1, BACKWARD },
      {0, BACKWARD },
      {3, BACKWARD },
    }
  };

  public PandaDriver(String ip) {
    this.ip = ip;
    
    // Initialize our OSC output stuff
    address = new NetAddress(ip, 779);
    message = new OscMessage("/shady/pointbuffer");
    
    try {
      socket = new DatagramSocket();
    } catch (Exception x) {
      throw new RuntimeException(x);
    }
    
    // Build the array of points, initialize all to nothing
    points = new int[PandaMapping.PIXELS_PER_BOARD];
    for (int i = 0; i < points.length; ++i) {
      points[i] = NO_POINT;
    }
  }
    
  public PandaDriver(String ip, Model model, PandaMapping _pm) {
    this(ip);
    pm = _pm;
    _model = model;
    // Ok, we are initialized, time to build the array if points in order to
    // send out. We start at the head of our point buffer, and work our way
    // down. This is the order in which points will be sent down the wire.
    int ci = -1;
    
    // Iterate through all our channelq s
    for (ChannelMapping channel : pm.channelList) {
      ++ci;
      int pi = ci * ChannelMapping.PIXELS_PER_CHANNEL;
      
      switch (channel.mode) {

        case ChannelMapping.MODE_CUBES:
          // We have a list of cubes per channel
          for (int rawCubeIndex : channel.objectIndices) {
            if (rawCubeIndex < 0) {
              // No cube here, skip ahead in the buffer
              pi += Cube.POINTS_PER_CUBE;
            } else {
              // The cube exists, check which way it is wired to
              // figure out the order of strips.
              Cube cube = model.getCubeByRawIndex(rawCubeIndex);
              int stripOrderIndex = 0;
              switch (cube.wiring) {
                case FRONT_LEFT: stripOrderIndex = 0; break;
                case FRONT_RIGHT: stripOrderIndex = 1; break;
                case REAR_LEFT: stripOrderIndex = 2; break;
                case REAR_RIGHT: stripOrderIndex = 3; break;
              }
              
              // TODO(mcslee): clean up, ordering always consistent now
              stripOrderIndex = 2;
              
              // Iterate through all the strips on the cube and add the points
              for (int stripIndex : CUBE_STRIP_ORDERINGS[stripOrderIndex]) {
                // We go backwards here... in the model strips go clockwise, but
                // the physical wires are run counter-clockwise
                pi = mapStrip(cube.strips.get(stripIndex), BACKWARD, points, pi);
              }
            }
          }
          break;
          
        case ChannelMapping.MODE_BASS:
          for (int[] config : BASS_STRIP_ORDERING) {
            pi = mapStrip(model.bassBox.strips.get(config[0]), config[1], points, pi);
            if (config.length >= 3) pi += config[2];            
          }
          break;
          
        case ChannelMapping.MODE_STRUTS_AND_FLOOR:
          for (int[] config : STRUT_STRIP_ORDERING) {
            pi = mapStrip(model.bassBox.struts.get(config[0]), config[1], points, pi);
            if (config.length >= 3) pi += config[2];
          }     
          for (int[] config : FLOOR_STRIP_ORDERING) {
            pi = mapStrip(model.boothFloor.strips.get(config[0]), config[1], points, pi);
            if (config.length >= 3) pi += config[2];
          }
          break;
          
        case ChannelMapping.MODE_SPEAKER:
          int [][] speakerStripOrdering;
          if (SPEAKER_STRIP_ORDERING == null) {
            // Copy the cube strip ordering
            int[] frontLeftCubeWiring = CUBE_STRIP_ORDERINGS[0];
            speakerStripOrdering = new int[frontLeftCubeWiring.length][];
            for (int i = 0; i < frontLeftCubeWiring.length; ++i) {
              speakerStripOrdering[i] = new int[] { frontLeftCubeWiring[0], BACKWARD }; 
            }
          } else {
            speakerStripOrdering = SPEAKER_STRIP_ORDERING[channel.objectIndices[0]];
          }
          for (int[] config : speakerStripOrdering) {
            Speaker speaker = model.speakers.get(channel.objectIndices[0]);
            pi = mapStrip(speaker.strips.get(config[0]), config[1], points, pi);
            if (config.length >= 3) pi += config[2];            
          }
          break;
          
        case ChannelMapping.MODE_NULL:
          // No problem, nothing on this channel!
          break;
          
        default:
          throw new RuntimeException("Invalid/unhandled channel mapping mode: " + channel.mode);
      }

    }
  }
  
  private int mapStrip(Strip s, int direction, int[] points, int pi) {
    return mapStrip(s, direction, points, pi, s.points.size());
  }
  
  private int mapStrip(Strip s, int direction, int[] points, int pi, int len) {
    if (direction == FORWARD) {
      int i = 0;
      for (LXPoint p : s.points) {
        points[pi++] = p.index;
        if (++i >= len) {
          break;
        }
      }
    } else if (direction == BACKWARD) {
      for (int i = len-1; i >= 0; --i) {
        points[pi++] = s.points.get(i).index;
      }
    } else {
      throw new RuntimeException("Unidentified strip mapping direction: " + direction);
    }
    return pi;
  }

  public PandaDriver setListener(Listener listener) {
    this.listener = listener;
    return this;
  }

  public void setEnabled(boolean enabled) {
    if (this.enabled != enabled) {
      this.enabled = enabled;
      println("PandaBoard/" + ip + ": " + (enabled ? "ON" : "OFF"));
      if (listener != null) {
        listener.onToggle(enabled);
      }
    }
  }
  
  public boolean isEnabled() {
    return this.enabled;
  }

  public void disable() {
    setEnabled(false);
  }
  
  public void enable() {
    setEnabled(true);
  }

  public void toggle() {
    setEnabled(!enabled);
  }

  private final int[] GRIZZLY_STRIP_ORDERING = new int[] {  9,  8, 11,  5,  4,  7,  6, 10, 14,  2,  1,  0,  3, 13, 12, 15 };

  public final void send(int[] colors) {
    if (!enabled) {
      return;
    }
    frameNum++;
    int len = 0;
    int packetNum = 0;
    for (ChannelMapping channel : pm.channelList) {
      for (int rawCubeIndex : channel.objectIndices) {
        if (rawCubeIndex > 0) {
          Cube cube = _model.getCubeByRawIndex(rawCubeIndex);
          
          // TODO(mcslee): clean this up, precompute paths
          for (int stripIndex : GRIZZLY_STRIP_ORDERING) {
            Strip strip = cube.strips.get(stripIndex);
            int stripLen = ((stripIndex == 9) || (stripIndex == 16)) ? 15 : 16;
            for (int i = stripLen-1; i >= 0; --i) {
              int c = colors[strip.points.get(i).index];
              byte r = (byte) ((c >> 16) & 0xFF);
              byte g = (byte) ((c >> 8) & 0xFF);
              byte b = (byte) ((c) & 0xFF);
              packet[len++] = (byte) 0; // alpha channel, unused but makes for 4-byte alignment
              packet[len++] = (byte) r;
              packet[len++] = (byte) g;
              packet[len++] = (byte) b;
            }
          }
          
//          for (LXPoint p : cube.points) {
//            int c = (p.index < 0) ? 0 : colors[p.index];
//            byte r = (byte) ((c >> 16) & 0xFF);
//            byte g = (byte) ((c >> 8) & 0xFF);
//            byte b = (byte) ((c) & 0xFF);
//            packet[len++] = (byte) 0; // alpha channel, unused but makes for 4-byte alignment
//            packet[len++] = (byte) r;
//            packet[len++] = (byte) g;
//            packet[len++] = (byte) b;
//          }
        }
      }
      // println("Packet number: " + packetNum);
      sendPacket(frameNum, packetNum++);
      len = 0;
    }
    // for (int index : points) {
    //   int c = (index < 0) ? 0 : colors[index];
    //   byte r = (byte) ((c >> 16) & 0xFF);
    //   byte g = (byte) ((c >> 8) & 0xFF);
    //   byte b = (byte) ((c) & 0xFF);
    //   packet[len++] = 0; // alpha channel, unused but makes for 4-byte alignment
    //   packet[len++] = r;
    //   packet[len++] = g;
    //   packet[len++] = b;

    //   // Flush once packet is full buffer size
    //   if (len >= packet.length) {
    //     sendPacket(packetNum++);
    //     len = 0;
    //   }
    // }

    // // Flush any remaining data
    // if (len > 0) {
    //   sendPacket(packetNum++);
    // }
  }
  

  private void sendPacket(int frameNum, int packetNum) {
    // println("Sending frame #" + frameNum + ", channel # " + packetNum);
    message.clearArguments();
    message.add(frameNum);
    message.add(0xDEADBEEF);
    message.add(packetNum);
    message.add(0xFEEDBEEF);
    message.add(packet.length);
    message.add(packet);
    message.add(0xBEFFFFEB);

    try {
      // OscP5.flush(message, address); // new DatagramSocket every time, no thanks
      byte[] bytes = message.getBytes();
      DatagramPacket packet = new DatagramPacket(bytes, bytes.length, address.inetaddress(), PORT);
      socket.send(packet);
    } catch (Exception x) {
      x.printStackTrace();
    }
  }
}