//
// based on mass/spring/spring_set originally by Mike Davis of lightcycle.org (and inspired by Soda Constructor)
//

float const_mass      = 1,     // not sure what this should be for Soda Constructor compatibility... 1.0?
      const_bounce    = -0.25, // I think this is surface_reflection
      const_friction  = 0.98,  // this is 1-environment.friction I think
      const_sfriction = 0.01,   // this is surface_friction, not sure about my implementation of it though
      const_gravityx  = 0,     // not used in Soda Constructor
      const_gravityy  = 0;     // environment gravity 

float amplitude_factor = 5.0; // I seem to need this to get Soda-like muscles

int mass_radius = 3;

Model model;

void setup(){
  model = new Model("daintywalker.xml"); // also try "dirkjiggler.xml" or even http://sodaplay.com/constructor/beta/daintywalker.xml 
  size((int)model.width, (int)model.height);
  const_gravityy = model.gravitydirection.equals("down") ? model.gravity : -model.gravity;
  const_friction = 1.0-model.friction;
  const_bounce = model.surface_reflection;
  const_sfriction = model.surface_friction;
  ellipseMode(CENTER_RADIUS);
  smooth();
} 

boolean gotNode = false;
int nodeGot = -1;

void loop(){
  background(255);
  model.draw();
  model.step();
  if (mousePressed) {
    if (!gotNode) {
      int mini = -1;
      float minsqd = MAX_FLOAT;
      for (int i = 0; i < model.nodes.length; i++) {
        float sqd = sq(mouseX-model.nodes[i].px)+ sq(mouseY-model.nodes[i].py);
        if (minsqd > sqd)  {
          minsqd = sqd;
          mini = i;
        }
      }
      if (minsqd < 50.0) {
        nodeGot = mini;
        gotNode = true;
      }
    }
    else {
      model.nodes[nodeGot].px = mouseX;
      model.nodes[nodeGot].py = mouseY;
      model.nodes[nodeGot].vx = 0;
      model.nodes[nodeGot].vy = 0;
    }
  }
  else {
    nodeGot = -1;
    gotNode = false;  
  }
}

float getFloat(String s) {
  return Float.parseFloat(s);
} 

class Model {

  // from soda's xml format...
  String comment;
  float width, height;
  float gravity, friction, springyness;
  float surface_friction, surface_reflection;
  float wave_amplitude, wave_phase, wave_speed;
  String gravitydirection, wavedirection, autoreverse;
  mass nodes[];
  spring links[];

  // "time" for oscillations
  float t = 0.0;

  Model(String file) {
    BElement root = parseXML(file);
    comment = root.children[0].content;
    width = getFloat(root.children[1].attributes[0].value);
    height = getFloat(root.children[1].attributes[1].value);
    gravity = getFloat(root.children[2].attributes[0].value);
    friction = getFloat(root.children[2].attributes[1].value);
    springyness = getFloat(root.children[2].attributes[2].value);
    surface_friction = getFloat(root.children[3].attributes[0].value);
    surface_reflection = getFloat(root.children[3].attributes[1].value);
    wave_amplitude = getFloat(root.children[4].attributes[0].value);
    wave_phase = getFloat(root.children[4].attributes[1].value);
    wave_speed = getFloat(root.children[4].attributes[2].value);
    gravitydirection = root.children[5].attributes[0].value;
    wavedirection = root.children[5].attributes[1].value;
    autoreverse = root.children[5].attributes[2].value;
    BElement nodesElem = root.children[6];
    Map tempNodes = new HashMap();
    nodes = new mass[nodesElem.children.length];
    for (int i = 0; i < nodesElem.children.length; i++) {
      mass m = new mass();
      m.px = getFloat(nodesElem.children[i].attributes[1].value);
      m.py = height-getFloat(nodesElem.children[i].attributes[2].value);
      m.vx = getFloat(nodesElem.children[i].attributes[3].value);
      m.vy = getFloat(nodesElem.children[i].attributes[4].value);
      m.mass = const_mass;
      String id = nodesElem.children[i].attributes[0].value;
      tempNodes.put(id, m);
      nodes[i] = m;
    }
    BElement linksElem = root.children[7];
    links = new spring[linksElem.children.length];
    for (int i = 0; i < linksElem.children.length; i++) {
      spring s;
      if (linksElem.children[i].name.equals("spring")) {
        s = new spring();
      }
      else {
        s = new muscle();
        ((muscle)s).amplitude = getFloat(linksElem.children[i].attributes[3].value);
        ((muscle)s).phase = getFloat(linksElem.children[i].attributes[4].value);
      }
      s.k = springyness;
      s.d = getFloat(linksElem.children[i].attributes[2].value);
      s.a = (mass)tempNodes.get(linksElem.children[i].attributes[0].value);
      s.b = (mass)tempNodes.get(linksElem.children[i].attributes[1].value);
      links[i] = s;
    }
    t = wave_phase;
  }
  void draw() {
    stroke(0, 0, 0, 215);
    for (int i = 0; i < links.length; i++) {
      spring s = links[i];
      s.draw();
    }
    stroke(0, 64, 0);
    fill(128, 255, 64);
    for (int i = 0; i < nodes.length; i++) {
      mass m = nodes[i];
      ellipse(m.px, m.py, mass_radius, mass_radius);
    }
  }
  void step() {
    // assess forces
    for (int i = 0; i < links.length; i++){
      spring s = links[i];
      float dist_offset = sqrt(sq(s.a.px - s.b.px) + sq(s.a.py - s.b.py)) - s.d;
      float angle = atan2(s.a.px - s.b.px, s.a.py - s.b.py);
      s.a.fx -= sin(angle) * s.k * dist_offset;
      s.a.fy -= cos(angle) * s.k * dist_offset;
      s.b.fx += sin(angle) * s.k * dist_offset;
      s.b.fy += cos(angle) * s.k * dist_offset;
    }
    // apply forces
    for (int i = 0; i < nodes.length; i++) {
      mass m = nodes[i];
      m.update();
    }
    for (int i = 0; i < links.length; i++) {
      spring s = links[i];
      s.update();
    }
    t += wave_speed * TWO_PI;
  }

  class mass {
    float px, py, vx, vy, ax, ay, mass = const_mass, fx, fy;
    void update(){
      vx += (const_gravityx + fx) / mass;
      vy += (const_gravityy + fy) / mass;
      px += vx;
      py += vy;
      fx = 0;
      fy = 0;
      vx *= const_friction;
      vy *= const_friction;
      if (px < mass_radius){
        px = mass_radius;
        vx *= const_bounce;
        vx *= const_sfriction;
      }
      if (py < mass_radius){
        py = mass_radius;
        vy *= const_bounce;
        vy *= const_sfriction;
      }
      if (px >= width-mass_radius){
        px = width - 1 - mass_radius;
        vx *= const_bounce;
        vx *= const_sfriction;
      }
      if (py >= height-mass_radius){
        py = height - 1 - mass_radius;
        vy *= const_bounce;
        vy *= const_sfriction;
      }
    }
  }

  class spring {
    mass a, b;
    float d = 10.0; // rest length
    float k = 1.0;    // strength
    void update() {}
    void draw() {
      line(a.px, a.py, b.px, b.py);
    }
  }

  class muscle extends spring {
    float amplitude, phase;
    void update() {
      d += amplitude_factor * amplitude * wave_amplitude * cos(t+(phase*TWO_PI));
    }
    void draw() {
      line(a.px, a.py, b.px, b.py);
      ellipse((a.px + b.px)/2, (a.py + b.py)/2, 2, 2);
    }
  }

} 

/////////////////////////////////////////////////
// XML Parsing
//
// Adapted from an article at DevX
// http://www.devx.com/xml/Article/10114
//
/////////////////////////////////////////////////

BElement parseXML(String location) {
  try {
    SimpleDOMParser parser = new SimpleDOMParser();
    BElement root = parser.parse(new BufferedReader(new InputStreamReader(loadStream(location))));
    root.finish();
    return root;
  }
  catch(Exception e) {
    println("error parsing XML");
    e.printStackTrace();
    return null;
  }
}

class BElement {

  String name;
  String content;

  BElement children[];
  BAttribute attributes[];

  boolean _finished = false;

  HashMap _attributes = new LinkedHashMap();
  ArrayList _children = new ArrayList();

  BElement(String name) {
    this.name = name;
    this.content = new String();
  }

  void finish() {
    if (!_finished) {
      println("finishing " + name);
      println("content " + content);
      Object kids[] = _children.toArray();
      children = new BElement[kids.length];
      for (int i = 0; i < kids.length; i++) {
        children[i] = (BElement)kids[i];
        children[i].finish();
      }
      Object attnames[] = _attributes.keySet().toArray();
      attributes = new BAttribute[attnames.length];
      for (int i = 0; i < attnames.length; i++) {
        attributes[i] = new BAttribute((String)attnames[i], (String)_attributes.get(attnames[i]));
      }
      _finished = true;
    }
  }
}

class BAttribute {
  String name, value;
  BAttribute(String name, String value) {
    this.name = name;
    this.value = value;
  }
}

class SimpleDOMParser {

  int[] cdata_start = {'<', '!', '[', 'C', 'D', 'A', 'T', 'A', '['};
  int[] cdata_end = {']', ']', '>'};

  Reader reader;
  Stack elements;
  BElement currentElement;

  SimpleDOMParser() {
    elements = new Stack();
    currentElement = null;
  }

   BElement parse(Reader reader) throws IOException {
     this.reader = reader;

     println("skip xml declaration or DocTypes");
     skipPrologs();

     while (true) {

       int index;
       String name;

       println("remove the prepend or trailing white spaces");
       String currentTag = readTag().trim();
       if (currentTag.startsWith("</")) {
         println("close tag");
         name = currentTag.substring(2, currentTag.length()-1);

         println("no open tag");
         if (currentElement == null) {
           throw new IOException("Got close tag '" + name + "' without open tag.");
         }

         println("close tag does not match with open tag");
         if (!name.equals(currentElement.name)) {
           throw new IOException("Expected close tag for '" + currentElement.name + "' but got '" + name + "'.");
         }

         if (elements.empty()) {
           println("document processing is over");
           return currentElement;
         } else {
           println("pop up the previous open tag");
           currentElement = (BElement)elements.pop();
         }
       } else {
         println("open tag or tag with both open and close tags");
         index = currentTag.indexOf(" ");
         if (index < 0) {
           println("tag with no attributes");
           if (currentTag.endsWith("/>")) {
             println("close tag as well");
             name = currentTag.substring(1, currentTag.length()-2);
             currentTag = "/>";
           } else {
             println("open tag");
             name = currentTag.substring(1, currentTag.length()-1);
             currentTag = "";
           }
         } else {
           println("tag with attributes");
           name = currentTag.substring(1, index);
           currentTag = currentTag.substring(index+1);
         }

         println("create new element");
         BElement element = new BElement(name);

         println("parse the attributes");
         boolean isTagClosed = false;
         while (currentTag.length() > 0) {
           println("remove the prepend or trailing white spaces");
           currentTag = currentTag.trim();

           if (currentTag.equals("/>")) {
             println("close tag");
             isTagClosed = true;
             break;
           } else if (currentTag.equals(">")) {
             println("open tag");
             break;
           }

           index = currentTag.indexOf("=");
           if (index < 0) {
             throw new IOException("Invalid attribute for tag '" + name + "'.");
           }

           println("get attribute name");
           String attributeName = currentTag.substring(0, index);
           currentTag = currentTag.substring(index+1);

           println("get attribute value");
           String attributeValue;
           boolean isQuoted = true;
           if (currentTag.startsWith("\"")) {
             index = currentTag.indexOf('"', 1);
           } else if (currentTag.startsWith("'")) {
             index = currentTag.indexOf('\'', 1);
           } else {
             isQuoted = false;
             index = currentTag.indexOf(' ');
             if (index < 0) {
               index = currentTag.indexOf('>');
               if (index < 0) {
                 index = currentTag.indexOf('/');
               }
             }
           }

           if (index < 0) {
             throw new IOException("Invalid attribute for tag '" + name + "'.");
           }

           if (isQuoted) {
             attributeValue = currentTag.substring(1, index);
           } else {
             attributeValue = currentTag.substring(0, index);
           }

           println("add attribute to the new element");
           element._attributes.put(attributeName, attributeValue);

           currentTag = currentTag.substring(index+1);
         }

         println("read the text between the open and close tag");
         if (!isTagClosed) {
           element.content = readText();
         }

         println("add new element as a child element of the current element");
         if (currentElement != null) {
           currentElement._children.add(element);
         }

         if (!isTagClosed) {
           if (currentElement != null) {
             elements.push(currentElement);
           }

           currentElement = element;
         } else if (currentElement == null) {
           println("only has one tag in the document");
           return element;
         }
       }
     }
   }

   int peek() throws IOException {
     reader.mark(1);
     int result = reader.read();
     reader.reset();

     return result;
   }

   void peek(int[] buffer) throws IOException {
     reader.mark(buffer.length);
     for (int i=0; i<buffer.length; i++) {
       buffer[i] = reader.read();
     }
     reader.reset();
   }

   void skipWhitespace() throws IOException {
     while (Character.isWhitespace((char)peek())) {
       reader.read();
     }
   }

   void skipProlog() throws IOException {
     // skip "<?" or "<!"
     reader.skip(2);

     while (true) {
       int next = peek();

       if (next == '>') {
         reader.read();
         break;
       } else if (next == '<') {
         // nesting prolog
         skipProlog();
       } else {
         reader.read();
       }
     }
   }

   void skipPrologs() throws IOException {
     while (true) {
       skipWhitespace();

       int[] next = new int[2];
       peek(next);

       if (next[0] != '<') {
         throw new IOException("Expected '<' but got '" + (char)next[0] + "'.");
       }

       if ((next[1] == '?') || (next[1] == '!')) {
         skipProlog();
       } else {
         break;
       }
     }
   }

   String readTag() throws IOException {
     skipWhitespace();

     StringBuffer sb = new StringBuffer();

     int next = peek();
     if (next != '<') {
       throw new IOException("Expected < but got " + (char)next);
     }

     sb.append((char)reader.read());
     while (peek() != '>') {
       sb.append((char)reader.read());
     }
     sb.append((char)reader.read());

     return sb.toString();
   }

   String readText() throws IOException {
     StringBuffer sb = new StringBuffer();

     int[] next = new int[cdata_start.length];
     peek(next);
     if (compareIntArrays(next, cdata_start) == true) {
       // CDATA
       reader.skip(next.length);

       int[] buffer = new int[cdata_end.length];
       while (true) {
         peek(buffer);

         if (compareIntArrays(buffer, cdata_end) == true) {
           reader.skip(buffer.length);
           break;
         } else {
           sb.append((char)reader.read());
         }
       }
     } else {
       while (peek() != '<') {
         sb.append((char)reader.read());
       }
     }

     return sb.toString();
   }

   boolean compareIntArrays(int[] a1, int[] a2) {
     if (a1.length != a2.length) {
       return false;
     }

     for (int i=0; i<a1.length; i++) {
       if (a1[i] != a2[i]) {
         return false;
       }
     }

     return true;
   }
 }