diff --git a/src/org/poly2tri/MonoToneMountain.scala b/src/org/poly2tri/MonoToneMountain.scala
index f3640d1..88700ba 100644
--- a/src/org/poly2tri/MonoToneMountain.scala
+++ b/src/org/poly2tri/MonoToneMountain.scala
@@ -115,8 +115,7 @@ class MonotoneMountain {
          if(valid(c)) convexPoints.enqueue(c)
 
 	   }    
-	   assert(size <= 3, "Triangulation bug")
-	   if(size == 3)lastTriangle
+	   assert(size <= 2, "Triangulation bug")
 	}
    }
  
diff --git a/src/org/poly2tri/Poly2Tri.scala b/src/org/poly2tri/Poly2Tri.scala
index e22b704..f06807b 100644
--- a/src/org/poly2tri/Poly2Tri.scala
+++ b/src/org/poly2tri/Poly2Tri.scala
@@ -55,10 +55,12 @@ object Poly2Tri {
 class Poly2TriDemo extends BasicGame("Poly2Tri") {
   
   var tesselator: Triangulator = null
+  var segments: ArrayBuffer[Segment] = null
   
   var quit = false
   var debug = false
   var drawMap = false
+  var drawSegs = false
   var hiLighter = 0
   
   def init(container: GameContainer) {
@@ -79,14 +81,17 @@ class Poly2TriDemo extends BasicGame("Poly2Tri") {
    if(debug) {
 	   val draw = if(drawMap) tesselator.trapezoidMap else tesselator.trapezoids
 	   for(t <- draw) {
+	     assert(t.rightPoint != t.leftPoint)
 	     val polygon = new Polygon()
 	     for(v <- t.vertices) {
 	       polygon.addPoint(v.x, v.y)
 	     }
-	     val lCirc = new Circle(t.leftPoint.x, t.leftPoint.y, 4)
-	     g.setColor(blue); g.draw(lCirc); g.fill(lCirc)
-	     val rCirc = new Circle(t.rightPoint.x, t.rightPoint.y, 4)
-	     //g.setColor(yellow); g.draw(rCirc); g.fill(rCirc)
+	     if(!drawMap) {
+	       val lCirc = new Circle(t.leftPoint.x, t.leftPoint.y, 4)
+	       g.setColor(blue); g.draw(lCirc); g.fill(lCirc)
+	       val rCirc = new Circle(t.rightPoint.x+5, t.rightPoint.y, 4)
+	       g.setColor(yellow); g.draw(rCirc); g.fill(rCirc)
+         }                          
 	     g.setColor(red)
 	     g.draw(polygon)
 	    }
@@ -98,12 +103,14 @@ class Poly2TriDemo extends BasicGame("Poly2Tri") {
     	if(t.size < 3) println("wtf")
         val triangle = new Polygon
         t.foreach(p => triangle.addPoint(p.x, p.y))
-        val color = if(i == hiLighter) blue else red
-        g.setColor(color)
+        g.setColor(red)
         g.draw(triangle)
-        i += 1
       }
-   } else {
+      val triangle = new Polygon
+      tesselator.triangles(hiLighter).foreach(p => triangle.addPoint(p.x, p.y))
+      g.setColor(blue)
+      g.draw(triangle)
+   } else if (debug && drawMap){
     for(mp <- tesselator.monoPolies) {
       val poly = new Polygon
       mp.foreach(p => poly.addPoint(p.x, p.y))
@@ -111,6 +118,12 @@ class Poly2TriDemo extends BasicGame("Poly2Tri") {
       g.draw(poly)
       }
     }
+   
+   if(drawSegs) {
+     g.setColor(green)
+     for(s <- segments) 
+         g.drawLine(s.p.x,s.p.y,s.q.x,s.q.y)
+   }
     
   }
   
@@ -135,6 +148,7 @@ class Poly2TriDemo extends BasicGame("Poly2Tri") {
     if(c == '1') {poly; hiLighter = 0}
     if(c == '2') {snake; hiLighter = 0}
     if(c == '3') {star; hiLighter = 0}
+    if(c == 's') drawSegs = !drawSegs
 
   }
   
@@ -158,7 +172,7 @@ class Poly2TriDemo extends BasicGame("Poly2Tri") {
     val p15 = Point(180,352)
     val p16 = Point(300,312)
     
-    val segments = new ArrayBuffer[Segment]
+    segments = new ArrayBuffer[Segment]
     segments += new Segment(p1, p2)
     segments += new Segment(p2, p3)
     segments += new Segment(p3, p4)
@@ -193,7 +207,7 @@ class Poly2TriDemo extends BasicGame("Poly2Tri") {
     val p9 = Point(231,161)
     val p10 = Point(321,161)
     
-    val segments = new ArrayBuffer[Segment]
+    segments = new ArrayBuffer[Segment]
     segments += new Segment(p1, p2)
     segments += new Segment(p2, p3)
     segments += new Segment(p3, p4)
@@ -226,7 +240,7 @@ class Poly2TriDemo extends BasicGame("Poly2Tri") {
     val p11 = Point(1,20)*scale+displace
     val p12 = Point(1,10)*scale+displace
     
-    val segments = new ArrayBuffer[Segment]
+    segments = new ArrayBuffer[Segment]
     segments += new Segment(p1, p2)
     segments += new Segment(p2, p3)
     segments += new Segment(p3, p4)
diff --git a/src/org/poly2tri/QueryGraph.scala b/src/org/poly2tri/QueryGraph.scala
index 172bc88..ffe1b39 100644
--- a/src/org/poly2tri/QueryGraph.scala
+++ b/src/org/poly2tri/QueryGraph.scala
@@ -49,7 +49,7 @@ class QueryGraph(var head: Node) {
 	      if(s > trapezoids(j).rightPoint) {
 	        trapezoids += trapezoids(j).upperRight
 	      } else {
-	          trapezoids += trapezoids(j).lowerRight
+	        trapezoids += trapezoids(j).lowerRight
 	      }
 	      j += 1
 	    }
diff --git a/src/org/poly2tri/Trapezoid.scala b/src/org/poly2tri/Trapezoid.scala
index 96a1894..71e7794 100644
--- a/src/org/poly2tri/Trapezoid.scala
+++ b/src/org/poly2tri/Trapezoid.scala
@@ -80,7 +80,7 @@ class Trapezoid(val leftPoint: Point, var rightPoint: Point, val top: Segment, v
     verts(1) = lineIntersect(bottom, leftPoint.x)
     verts(2) = lineIntersect(bottom, rightPoint.x)
     verts(3) = lineIntersect(top, rightPoint.x)
-    return verts
+    verts
   }
   
   def lineIntersect(s: Segment, x: Float) = {
diff --git a/src/org/poly2tri/TrapezoidalMap.scala b/src/org/poly2tri/TrapezoidalMap.scala
index 17915a8..0a81b1f 100644
--- a/src/org/poly2tri/TrapezoidalMap.scala
+++ b/src/org/poly2tri/TrapezoidalMap.scala
@@ -48,13 +48,13 @@ class TrapezoidalMap {
   
   // Add a trapezoid to the map
   def add(t: Trapezoid) {
-    assert(t != null)
+    assert(t != null, "Bad value")
     map += t
   }
   
   // Remove a trapezoid from the map
   def remove(t: Trapezoid) {
-    assert(t != null)
+    assert(t != null, "Bad value")
     map -=t
   }
   
@@ -93,12 +93,10 @@ class TrapezoidalMap {
     
     assert(s.p.x < s.q.x)
     
-    val rp = if(s.q.x == t.rightPoint.x) s.q else t.rightPoint
-    
     val trapezoids = new ArrayBuffer[Trapezoid]
     trapezoids += new Trapezoid(t.leftPoint, s.p, t.top, t.bottom)
-    trapezoids += new Trapezoid(s.p, rp, t.top, s)
-    trapezoids += new Trapezoid(s.p, rp, s, t.bottom)
+    trapezoids += new Trapezoid(s.p, t.rightPoint, t.top, s)
+    trapezoids += new Trapezoid(s.p, t.rightPoint, s, t.bottom)
    
     trapezoids(0).update(t.upperLeft, t.lowerLeft, trapezoids(1), trapezoids(2))
     trapezoids(1).update(trapezoids(0), null, t.upperRight, null)
@@ -118,20 +116,17 @@ class TrapezoidalMap {
     assert(s.p.x != s.q.x)
     assert(s.p.x < s.q.x)
     
-    val lp = if(s.p.x == t.leftPoint.x) s.p else t.leftPoint
-    val rp = if(s.q.x == t.rightPoint.x) s.q else t.rightPoint
-    
     val topCross = (tCross == t.top)
     val bottomCross = (bCross == t.bottom)
     
     val trapezoids = new ArrayBuffer[Trapezoid]
-    trapezoids += {if(topCross) t.upperLeft else new Trapezoid(lp, rp, t.top, s)}
-    trapezoids += {if(bottomCross) t.lowerLeft else new Trapezoid(lp, rp, s, t.bottom)}
+    trapezoids += {if(topCross) t.upperLeft else new Trapezoid(t.leftPoint, t.rightPoint, t.top, s)}
+    trapezoids += {if(bottomCross) t.lowerLeft else new Trapezoid(t.leftPoint, t.rightPoint, s, t.bottom)}
     
     if(topCross) {
       trapezoids(0).upperRight = t.upperRight
       if(t.upperRight != null) t.upperRight.upperLeft = trapezoids(0)
-      trapezoids(0).rightPoint = rp
+      trapezoids(0).rightPoint = t.rightPoint
     } else {
       trapezoids(0).update(t.upperLeft, s.above, t.upperRight, null)
     }
@@ -139,7 +134,7 @@ class TrapezoidalMap {
     if(bottomCross) {
       trapezoids(1).lowerRight = t.lowerRight
       if(t.lowerRight != null) t.lowerRight.lowerLeft = trapezoids(1)
-      trapezoids(1).rightPoint = rp
+      trapezoids(1).rightPoint = t.rightPoint
     } else {
       trapezoids(1).update(s.below, t.lowerLeft, null, t.lowerRight)
     }
@@ -157,15 +152,13 @@ class TrapezoidalMap {
   def case4(t: Trapezoid, s: Segment) = {
     
     assert(s.p.x < s.q.x)
-    
-    val lp = if(s.p.x == t.leftPoint.x) s.p else t.leftPoint
-    
+       
     val topCross = (tCross == t.top)
     val bottomCross = (bCross == t.bottom)
-    
+
     val trapezoids = new ArrayBuffer[Trapezoid]
-    trapezoids += {if(topCross) t.upperLeft else new Trapezoid(lp, s.q, t.top, s)}
-    trapezoids += {if(bottomCross) t.lowerLeft else new Trapezoid(lp, s.q, s, t.bottom)}
+    trapezoids += {if(topCross) t.upperLeft else new Trapezoid(t.leftPoint, s.q, t.top, s)}
+    trapezoids += {if(bottomCross) t.lowerLeft else new Trapezoid(t.leftPoint, s.q, s, t.bottom)}
     trapezoids += new Trapezoid(s.q, t.rightPoint, t.top, t.bottom)
     
     if(topCross) {
@@ -210,7 +203,7 @@ class TrapezoidalMap {
     val top = new Segment(Point(min.x, max.y), Point(max.x, max.y))
     val bottom = new Segment(Point(min.x, min.y), Point(max.x, min.y))
     val left = bottom.p
-    val right = top.q
+    val right = bottom.q
     
     return new Trapezoid(left, right, top, bottom)
   }
diff --git a/src/org/poly2tri/Triangulator.scala b/src/org/poly2tri/Triangulator.scala
index 6f408b6..5b842e3 100644
--- a/src/org/poly2tri/Triangulator.scala
+++ b/src/org/poly2tri/Triangulator.scala
@@ -75,7 +75,7 @@ class Triangulator(segments: ArrayBuffer[Segment]) {
       }
       trapezoidalMap reset
     }
-    
+  
     // Mark outside trapezoids
     trapezoidalMap.map.foreach(markOutside)
     
@@ -111,7 +111,6 @@ class Triangulator(segments: ArrayBuffer[Segment]) {
   // Initialize trapezoidal map and query structure
   private val trapezoidalMap = new TrapezoidalMap
   private val boundingBox = trapezoidalMap.boundingBox(segments)
-  trapezoidalMap add boundingBox
   private val queryGraph = new QueryGraph(Sink.init(boundingBox))
   private val xMonoPoly = new ArrayBuffer[MonotoneMountain]