int xSize           = 400;
int ySize           = 400;
int totalParticles  = 2;

Particle[] particle;

int counter = 0;

float xMouse, yMouse;
float rotationVar;
float elevation, azimuth, twist, distance;

void setup(){
  size(xSize, ySize);
  background(255);
  ellipseMode(CENTER_DIAMETER);
  particle  = new Particle[totalParticles];
  
  elevation = radians(15.0f);
  azimuth = radians(0.0f);
  distance = 400.0f;
  
  particle[0] = new Particle(100, 75, 85, 0);
  particle[1] = new Particle(50, 100, 33, 1);
}


float angle;
float dist;
float xv, yv, zv;

float zAngle;
float totalDist;

void loop(){
  background(255);
  doCamera();
  drawGrid();
  
  particle[0].render();
  particle[1].render();
  
  // draw xy line in red
  angle        = findAngle(particle[0].x, particle[0].y, particle[1].x, particle[1].y);
  dist         = findDistance(particle[0].x, particle[0].y, particle[1].x, particle[1].y);
  xv           = cos(angle) * dist;
  yv           = sin(angle) * dist;
  stroke(255,0,0);
  line(particle[0].x, particle[0].y, 0, particle[0].x - xv, particle[0].y - yv, 0);
  
  // draw yz line in green
  angle        = findAngle(particle[0].y, particle[0].z, particle[1].y, particle[1].z);
  dist         = findDistance(particle[0].y, particle[0].z, particle[1].y, particle[1].z);
  yv           = cos(angle) * dist;
  zv           = sin(angle) * dist;
  stroke(0,255,0);
  line(0, particle[0].y, particle[0].z, 0, particle[0].y - yv, particle[0].z - zv);
  
  // draw xz line in blue
  angle        = findAngle(particle[0].x, particle[0].z, particle[1].x, particle[1].z);
  dist         = findDistance(particle[0].x, particle[0].z, particle[1].x, particle[1].z);
  xv           = cos(angle) * dist;
  zv           = sin(angle) * dist;
  stroke(0,0,255);
  line(particle[0].x, 0, particle[0].z, particle[0].x - xv, 0, particle[0].z - zv);
  
  // draw connecting line
  dist         = findDistance(particle[0].x, particle[0].y, particle[1].x, particle[1].y);
  zAngle       = findAngle(particle[0].x, particle[0].y, dist, particle[1].z);
  totalDist    = findDistance(particle[0].x, particle[0].y, dist, particle[1].z);
  zv           = sin(zAngle) * totalDist;

  stroke(0);
  line(particle[0].x, particle[0].y, particle[0].z, particle[0].x - xv, particle[0].y - yv, particle[0].z - zv);
  noStroke();
}


void drawGrid(){
  stroke(255,0,0);
  fill(255,0,0,25);
  beginShape(QUADS);
  vertex(-200,-200,0);
  vertex(200,-200,0);
  vertex(200,200,0);
  vertex(-200,200,0);
  endShape();
  line(-200,0,0,200,0,0);
  
  stroke(0,255,0);
  fill(0,255,0,25);
  beginShape(QUADS);
  vertex(0,-200,-200);
  vertex(0,200,-200);
  vertex(0,200,200);
  vertex(0,-200,200);
  endShape();
  line(0,-200,0,0,200,0);

  stroke(0,0,255);
  fill(0,0,255,25);
  beginShape(QUADS);
  vertex(-200,0,-200);
  vertex(200,0,-200);
  vertex(200,0,200);
  vertex(-200,0,200);
  endShape();
  line(0,0,-200,0,0,200);
  noStroke();
}

void doCamera(){
  xMouse -= (xMouse - (width/2 - mouseX)) * .2f;
  yMouse -= (yMouse - (height/2 - mouseY)) * .2f;
  beginCamera();
  perspective(60.0f, (float)xSize / (float)ySize, 1.0f, 500);
  translate(0.0, 0.0, -distance);
  
  twist = radians(xMouse / 2.0f);
  elevation = radians(yMouse / 2.0f);
  
  rotateY(-twist);
  rotateX(elevation);
  rotateZ(-azimuth);
  
  endCamera();
}



class Particle {
  int index;
  
  float x;
  float y;
  float z;
  
  float xv;
  float yv;
  float zv;

  float myVel;
  float myAngle;
  float myDist;

  Particle(float xSent, float ySent, float zSent, int sentIndex){
    x = xSent;
    y = ySent;
    z = zSent;

    index = sentIndex;
  }
  
  void render(){
    stroke(0);
    noFill();
    push();
    translate(x,y,z);
    box(20);
    pop();
    
    stroke(0,255,0,100);
    line(x,y,z,0,y,z);
    stroke(0,0,255,100);
    line(x,y,z,x,0,z);
    stroke(255,0,0,100);
    line(x,y,z,x,y,0);
    noStroke();
  }
}

float findDistance(float x1, float y1, float x2, float y2){
  float xd = x1 - x2;
  float yd = y1 - y2;
  float td = sqrt(xd * xd + yd * yd);
  return td;
}
  
float findAngle(float x1, float y1, float x2, float y2){
  float xd = x1 - x2;
  float yd = y1 - y2;
  float t = atan2(yd,xd);
  return t;
}

//