float selRad = 5f;

void mousePressed() {
  int selectedSizeBefore = selected.size();
  selected.clear(); redraw = true;
  if (!beginNewPolygon) {
    for (Polygon poly : polygons) {
      if (poly.selectable) {
        boolean gotVec = false;
        for (Vector vec : poly.vertices) {
          if (dist(mouseX, mouseY, vec.x, vec.y) < selRad) {
            selected.add(vec); gotVec = true;
          }
        }
        if (!gotVec) {
          if (poly.pointInPolygon((float)mouseX, (float)mouseY)) {
            for (Vector vec : poly.vertices) selected.add(vec);
            selected.add(poly); redraw = true;
          }
        }
      }
    }
  } else {
    if (newPolygon == null) newPolygon = new Polygon();
    newPolygon.vertices.add(new Vector(mouseX, mouseY));
  }
}

void mouseReleased() {redraw = true;}
  
void mouseDragged() {
  for (Object sel : selected) {
    if (sel.getClass() == Vector.class) {
      Vector vec = (Vector)sel;
      vec.add(new PVector((float)(mouseX - pmouseX), (float)(mouseY - pmouseY))); redraw = true;
    }
  }
}

void keyTyped() {
  if (key == 'n') {beginNewPolygon = true; redraw = true;}
  if (key == DELETE || key == BACKSPACE) {
    Object remove = null;
    for (Object sel : selected) {
      if (sel.getClass() == Polygon.class) {polygons.remove(sel); remove = sel; redraw = true;}
    }
    if (remove != null) selected.remove(remove);
  }
}

public class Polygon {
  ArrayList<Vector> vertices;
  boolean closed = false;
  boolean visible = true;
  boolean selectable = true;
  
  public Polygon() {
    vertices = new ArrayList<Vector>();
  }
  
  public Polygon(Vector... vec) {
    vertices = new ArrayList<Vector>();
    for (Vector vv : vec) {
      vertices.add(vv);
      vv.polygon = this;
    }
  }
  
  public ArrayList<Line2D> getLines2D() {
    ArrayList<Line2D> result = new ArrayList<Line2D>();
    Vector last = vertices.get(vertices.size() - 1);
    for (Vector vec : vertices) {
      if (closed || vertices.indexOf(vec) > 0) result.add(new Line2D(vec.getVec2D(), last.getVec2D()));
      last = vec;
    }
    return result;
  }
  
  public void display() {
    if (visible) {
    strokeWeight(3);
    noFill();
    if (selected.contains(this)) {stroke(255, 0, 0);}
    else {stroke(0);}
    if (vertices.size() > 0) {
      beginShape();
      for (Vector vec : vertices) vertex(vec.x, vec.y);
      //vertex(vertices.get(0).x, vertices.get(0).y);
      endShape();
      
      for (Vector vec : vertices) vec.display();
    }
    }
  }
  
  public boolean pointInPolygon(float x, float y) {
    return pointInPolygon(new Vector(x, y));
  }
  
  public boolean pointInPolygon(Vector iVec1) {
    Vector iVec2 = new Vector(iVec1.x + random(200) + 10000, iVec1.y + random(-200, 200));
    int ic = 0;
    if (vertices.size() > 2) {
      Vector prevVec = vertices.get(vertices.size() - 1);
      for (Vector vec : vertices) {
        if (intersection(iVec1, iVec2, vec, prevVec) != null) ic++;
        prevVec = vec;
      }
    }

    if (round((float)ic / 2) == (float)ic / 2) return false;
    else return true;
  }
}


public Vector intersection(Vector l1v1, Vector l1v2, Vector l2v1, Vector l2v2) {
  float x1 = l1v1.x;
  float y1 = l1v1.y;
  float x2 = l1v2.x;
  float y2 = l1v2.y;
  float x3 = l2v1.x;
  float y3 = l2v1.y;
  float x4 = l2v2.x;
  float y4 = l2v2.y;
  
  float tol = 0.01f;
  
  float d = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
  if (d == 0) return null;
    
  float xr = ((x3 - x4) * (x1 * y2 - y1 * x2) -
              (x1 - x2) * (x3 * y4 - y3 * x4)) / d;
  float yr = ((y3 - y4) * (x1 * y2 - y1 * x2) -
              (y1 - y2) * (x3 * y4 - y3 * x4)) / d;
  
  if (dist(x1, y1, xr, yr) + dist(x2, y2, xr, yr) - dist(x1, y1, x2, y2) < tol &&
      dist(x3, y3, xr, yr) + dist(x4, y4, xr, yr) - dist(x3, y3, x4, y4) < tol)
    return new Vector(xr, yr);
  else return null;
}


public class Vector extends PVector {
  Polygon polygon;
  float value = 0;
  int count = 0;
  boolean fixed = false;
  boolean blind = false;
  
  public Vector() {
    super();
  }
  public Vector(float x, float y) {
    super(x, y);
  }
  
  public Vec2D getVec2D() {
    return new Vec2D(x, y);
  }
  
  public void display() {
    pushStyle();
    if (selected.contains(this)) {
      stroke(255, 0, 0);
      strokeWeight(8);
    } else {
      stroke(0);
      strokeWeight(6);
    }
    point(x, y);
    popStyle();
  }
}

public class Agent {
	//the position of the Agent
	Vec3D pos;
	//the velocity of the Agent
	Vec3D vel;

        Vec3D lastPos;
        float speed = random(8, 16); //in meter per second
        boolean displayFOV = true;
        float fov = PI / 0.8f; //1.0001f;
        float fovRes = 15;
        float agility = random(0.45f, 1f);
        float vision = random(40, 240);
        
        int col = colorGradient((vision - 40) / 200, colors);//color((vision - 80) / 300 * 255, 0, 255);
        ArrayList<Vec2D> viewVectors;

        Vec2D myDir = null;
        
        public Agent() {
          while (true) {
            boolean isIn = false;
            pos = new Vec3D(random(0, width), 0, random(0, height));
            for (Polygon poly : polygons) {
              if (poly.visible) {
                if (poly.pointInPolygon(pos.x, pos.z)) {
                  isIn = true;
                }
              }
              if (isIn) break;
            }
            if (!isIn) break;
          }
          
          lastPos = pos.copy();
          vel = new Vec3D(random(-0.2f, 0.2f), 0, random(-0.2f, 0.2f)); //new Vec3D(p.random(-1f, 1f), 0, p.random(-1f, 1f));
        }
        
        Vec2D get2D(Vec3D newPos) {
          return new Vec2D(newPos.x, newPos.z);
	}

        public void update() {
          
          viewVectors = new ArrayList<Vec2D>();
          for (float ii = 0; ii <= fov; ii += fov / fovRes) {
            for (float i = -1f; i < 2; i += 2) {
              float iii = ii + random(-fov / fovRes / 2, fov / fovRes / 2);
              Vec2D tempVec = get2D(vel).copy().normalizeTo(vision * (1 - iii / fov * 0.5f));
              tempVec.rotate(i * iii);
              viewVectors.add(tempVec);
              
              if (ii == 0) break;
            }
          }
          
          
          
          //intersection ... neu Machen!!!!!!!!!!!!!!!!!!!
          HashMap<Vec2D, Vec2D> toReplace = new HashMap<Vec2D, Vec2D>();
          for (Vec2D vv : viewVectors) {
            //for (Line2D that : borders) {
            for (Polygon pp : polygons) {
              for (Line2D that : pp.getLines2D()) {
              Line2D.LineIntersection inter = that.intersectLine(new Line2D(get2D(pos), get2D(pos).copy().addSelf(vv)));
              
              if (inter.getType() == Line2D.LineIntersection.Type.INTERSECTING) {
                if (get2D(pos).distanceTo(inter.getPos().copy()) < vv.magnitude()) {
                  if (!toReplace.containsKey(vv)) toReplace.put(vv, inter.getPos().subSelf(get2D(pos)));
                  else {
                    if (get2D(pos).distanceTo(inter.getPos().copy()) < toReplace.get(vv).magnitude()) toReplace.put(vv, inter.getPos().subSelf(get2D(pos)));
                  }
                }
              }
              }
            }
          }
          
          for (Vec2D rr : toReplace.keySet()) {viewVectors.remove(rr); viewVectors.add(toReplace.get(rr));}
          
          Vec2D myNewDirection = null;
          float longestVec = 0;
          
          for (Vec2D vv : viewVectors) {if (vv.magnitude() > longestVec) {myNewDirection = vv; longestVec = vv.magnitude();}}
          
          vel.normalize();
          if (myNewDirection != null) {Vec3D myND = new Vec3D(myNewDirection.x, 0, myNewDirection.y).normalizeTo(agility); vel.addSelf(myND); myDir = myNewDirection;}
          
          vel.normalizeTo(speed * minutesPerFrame / (float)intermediate);
          pos.addSelf(vel);
        }
        
        public void display() {
          pushStyle();
          stroke(col, 12); strokeWeight(4); line(pos.x, pos.z, lastPos.x, lastPos.z);
          lastPos = pos.copy();
          //stroke(255, 0, 0); strokeWeight(1);
          //for (Vec2D vv : viewVectors) {if (vv == myDir) strokeWeight(4); else strokeWeight(1); line(pos.x, pos.z, pos.x + vv.x, pos.z + vv.y);}
          popStyle();
        }
        
        
  int colorGradient(float value, int[] colGrad) {
    float d = value;
    int ci = floor(d*((float)colGrad.length - 1));
    int cj = ci + 1;
    
    value = PApplet.abs(value);
    if (ci < colGrad.length - 1) {
      float d1 = d * (float)(colGrad.length - 1) - (float)ci;
      float d2 = d *(float)(colGrad.length - 1) - (float)cj;
      
      int a = (int)(d1 * alpha(colGrad[cj]) + abs(d2) * alpha(colGrad[ci]));
      int r = (int)(d1 * red(colGrad[cj]) + abs(d2) * red(colGrad[ci]));
      int g = (int)(d1 * green(colGrad[cj]) + abs(d2) * green(colGrad[ci]));
      int b = (int)(d1 * blue(colGrad[cj]) + abs(d2) * blue(colGrad[ci]));
      
      a = a << 24;
      r = r << 16;
      g = g << 8;
      
      return a | r | g | b;
      
    } else {
      return colGrad[colGrad.length - 1];
    }
  }
}

/**
  * Nils Seifert & Michael Muehlhaus
  *
  * www.muehlseife.de
  *
  * you can select & move the Vectors and Polygons
  * for drawing a new polygon, hold 'n'
**/

ArrayList<Polygon> polygons;
ArrayList<Object> selected;

Polygon newPolygon;
boolean beginNewPolygon = false;
boolean redraw = true;

int[] colors = {color(0, 0, 255), color(255, 0, 255), color(255, 0, 0), color(255, 255, 0), color(0, 255, 0)};

ArrayList<Agent> a;
float minutesPerFrame = 1f;

int intermediate = 1;
            
void setup() {
  size(800, 600, JAVA2D); smooth();
   
  polygons = new ArrayList<Polygon>();
  selected = new ArrayList<Object>();
   
  polygons.add(new Polygon(new Vector(0, 0),
                           new Vector(0, height),
                           new Vector(width, height),
                           new Vector(width, 0)));
  polygons.get(0).visible = false;
  polygons.get(0).selectable = false;
  polygons.get(0).closed = true;
  
  polygons.add(new Polygon(new Vector(160, 0),
                           new Vector(160, 220),
                           new Vector(320, 380)));
   
  polygons.add(new Polygon(new Vector(460, 600),
                           new Vector(460, 450),
                           new Vector(640, 450),
                           new Vector(640, 320)));
                           
  a = new ArrayList<Agent>();
  for (int i = 0; i < 600; i++) a.add(new Agent());                    
}

void draw() {
  //background(60, 80, 120);
  if (!keyPressed || (keyPressed && key != 'n')) {
    if (beginNewPolygon = true) {
      if (newPolygon != null) if (newPolygon.vertices.size() > 2) {polygons.add(newPolygon);}
    }
    beginNewPolygon = false; newPolygon = null;
  } else redraw = true;
  if (redraw) {
    background(255);
    //for (Polygon poly : polygons) {poly.display();}
    redraw = false;
    
    a = new ArrayList<Agent>();
    for (int i = 0; i < 600; i++) a.add(new Agent());
  }
  for (Polygon poly : polygons) {poly.display();}
  textAlign(LEFT, TOP);
  if (beginNewPolygon) {
    //fill(0); text("draw polygon by clicking left mouse button", 4, 4);
    
    if (newPolygon != null) if (newPolygon.vertices.size() > 0) {
      strokeWeight(1.5);
      noFill(); stroke(0, 125);
      beginShape();
      for (Vector vec : newPolygon.vertices) vertex(vec.x, vec.y);
      vertex(mouseX, mouseY);
      //vertex(newPolygon.vertices.get(0).x, newPolygon.vertices.get(0).y);
      endShape();
    }
  } //else {fill(0); text("hold 'n' for drawing new polygon", 4, 4);}
  
  for (int i = 0; i < intermediate; i++) for (Agent aa : a) aa.update();
  
  for (Agent aa : a) aa.display();
}