int p = 30;
int n = 5;
float[] nodesX = new float[n*n];
float[] nodesY = new float[n*n];
int scale;
float d = 0;
float xc = p-1;
float yc = p-1;
float xmc = 1;
float ymc = 1;

void setup(){
  size(720, 720);
  background(0);
  stroke(255);
  strokeWeight(1);

  scale = height/p;

  setNodes();
}

void draw(){
  background(0);

  lattice();
  display();
  update();
}

void mouseClicked(){
  update();
}

void update(){
  //background(0);

  float x = (int) random(xmc, xc)*scale;
  float y = (int) random(ymc, yc)*scale;
  int i = distance(x, y);
  float r1 = 0.05;
  float r2 = 0.01;

  fill(255, 122, 122);
  ellipse(x, y, 10, 10);
  fill(255);

  stroke(255, 0, 255);
  line(x, y, nodesX[i], nodesY[i]);
  stroke(255);

  for(int j = 0; j < 1; j++){
    if(nodesX[i] < x){
      nodesX[i] += r1*d;
      if(i+1 < nodesX.length){
        if(nodesX[i+1] < x) nodesX[i+1] += r2*d;
        else if(nodesX[i+1] > x) nodesX[i+1] -= r2*d;
      }
      if(i-1 >= 0){
        if(nodesX[i-1] < x) nodesX[i-1] += r2*d;
        else if(nodesX[i-1] > x) nodesX[i-1] -= r2*d;
      }
      if(i+n < nodesX.length){
        if(nodesX[i+n] < x) nodesX[i+n] += r2*d;
        else if(nodesX[i+n] > x) nodesX[i+n] -= r2*d;
      }
      if(i-n >= 0){
        if(nodesX[i-n] < x) nodesX[i-n] += r2*d;
        else if(nodesX[i-n] > x) nodesX[i-n] -= r2*d;
      }
    }else if(nodesX[i] > x){
      nodesX[i] = nodesX[i]-r1*d;
      if(i+1 < nodesX.length){
        if(nodesY[i+1] < x) nodesY[i+1] += r2*d;
        else if(nodesY[i+1] > x) nodesY[i+1] -= r2*d;
      }
      if(i-1 >= 0){
        if(nodesY[i-1] < x) nodesY[i-1] += r2*d;
        else if(nodesY[i-1] > x) nodesY[i-1] -= r2*d;
      }
      if(i+n < nodesX.length){
        if(nodesY[i+n] < x) nodesY[i+n] += r2*d;
        else if(nodesY[i+n] > x) nodesY[i+n] -= r2*d;
      }if(i-n >= 0){
        if(nodesY[i-n] < x) nodesY[i-n] += r2*d;
        else if(nodesY[i-n] > x) nodesY[i-n] -= r2*d;
      }
    }
    if(nodesY[i] < y){
      nodesY[i] += r1*d;
      if(i+1 < nodesY.length){
        if(nodesY[i+1] < y) nodesY[i+1] += r2*d;
        else if(nodesY[i+1] > y) nodesY[i+1] -= r2*d;
      }
      if(i-1 >= 0){
        if(nodesY[i-1] < y) nodesY[i-1] += r2*d;
        else if(nodesY[i-1] > y) nodesY[i-1] -= r2*d;
      }
      if(i+n < nodesY.length){
        if(nodesY[i+n] < y) nodesY[i+n] += r2*d;
        else if(nodesY[i+n] > y) nodesY[i+n] -= r2*d;
      }
      if(i-n >= 0){
        if(nodesY[i-n] < y) nodesY[i-n] += r2*d;
        else if(nodesY[i-n] > y) nodesY[i-n] -= r2*d;
      }
    }else if(nodesY[i] > y){
      nodesY[i] = nodesY[i]-r1*d;
      if(i+1 < nodesY.length){
        if(nodesY[i+1] < y) nodesY[i+1] += r2*d;
        else if(nodesY[i+1] > y) nodesY[i+1] -= r2*d;
      }
      if(i-1 >= 0){
        if(nodesY[i-1] < y) nodesY[i-1] += r2*d;
        else if(nodesY[i-1] > y) nodesY[i-1] -= r2*d;
      }
      if(i+n < nodesY.length){
        if(nodesY[i+n] < y) nodesY[i+n] += r2*d;
        else if(nodesY[i+n] > y) nodesY[i+n] -= r2*d;
      }if(i-n >= 0){
        if(nodesY[i-n] < y) nodesY[i-n] += r2*d;
        else if(nodesY[i-n] > y) nodesY[i-n] -= r2*d;
      }
    }
  }
}

int distance(float x, float y){
  int min = 0;

  for(int i = 0; i < nodesX.length; i++){
    if(i == 0){
      d = sqrt(pow(x - nodesX[i], 2) + pow(y - nodesY[i], 2));
      min = i;
    }else if(sqrt(pow(x - nodesX[i], 2) + pow(y - nodesY[i], 2)) < d){
      d = sqrt(pow(x - nodesX[i], 2) + pow(y - nodesY[i], 2));
      min = i;
    }
  }

  return min;
}

void display(){
  for(int i = 0; i < nodesX.length; i++){
    ellipse(nodesX[i], nodesY[i], 10, 10);

    stroke(0, 255, 0);
    if(i+1 < nodesX.length && (i+1)%n != 0){
      line(nodesX[i], nodesY[i], nodesX[i+1], nodesY[i+1]);
    }
    if(i+n < nodesX.length){
      line(nodesX[i], nodesY[i], nodesX[i+n], nodesY[i+n]);
    }
    stroke(255);
  }

  fill(0, 0, 255);
  ellipse(xmc*scale, ymc*scale, 10, 10);
  ellipse(xmc*scale, yc*scale, 10, 10);
  ellipse(xc*scale, yc*scale, 10, 10);
  ellipse(xc*scale, ymc*scale, 10, 10);
  fill(255);
}

void setNodes(){
  for(int i = 0; i < nodesX.length; i++){
    nodesX[i] = (int) random(1, p-1)*scale;
    nodesY[i] = (int) random(1, p-1)*scale;
  }
}

void lattice(){
  for(int i = scale; i <= width-scale; i+=scale){
    line(i, scale, i, height-scale);
  }
  for(int i = scale; i <= height-scale; i+=scale){
    line(scale, i, width-scale, i);
  }
}