//based on github.com/TariqSaiyad/Double-Pendulum

float angDiff = 0.0001;

PGraphics canvas;

Pendulum p1 = new Pendulum(0*angDiff, color(255, 0, 0));
Pendulum p2 = new Pendulum(1*angDiff, color(0, 255, 0));
Pendulum p3 = new Pendulum(2*angDiff, color(0, 0, 255));

void setup() {
  size(500, 500);
  blendMode(ADD);
}

void draw() {
  background(0);
  p1.update();
  p2.update();
  p3.update();
}

class Pendulum {

  float gravity=1;

  float x_offset = 250;
  float y_offset = 200;

  float r1 = 100;
  float r2 = 100;

  float mass1 = 20;
  float mass2 = 10;

  float a1=(PI/2);
  float a2=(PI/4);

  float a1_v = 0.15;
  float a2_v = 0;

  float a1_a=0;
  float a2_a=0;
  color c;
  Pendulum(float angle, color c_) {
    a1=angle;
    c = c_;
  }



  void update() {
    calculateAngles();

    //background(55);
    //image(canvas, 0, 0);
    stroke(c);
    fill(c);
    strokeWeight(2);
    // translate(400, 250);

    //calculate lines for pendulum.
    float x1 = r1*sin(a1);
    float y1 = r1*cos(a1);

    float x2 =x1+ r2*sin(a2);
    float y2 = y1+ r2*cos(a2);

    //draw the lines and pendulum.

    line(x_offset, y_offset, x1+x_offset, y1+y_offset);
    ellipse(x1+x_offset, y1+y_offset, mass2, mass2);
    line(x1+x_offset, y1+y_offset, x2+x_offset, y2+y_offset);
    ellipse(x2+x_offset, y2+y_offset, mass2, mass2);

    //change velocity by acceleration.
    a1_v += a1_a;
    a2_v+= a2_a;


    //change angles by velocity.
    a1+=a1_v;
    a2+=a2_v;
  }

  void calculateAngles() {

    //angle 1.
    float num1 = -gravity*(2* mass1+mass2)*sin(a1);
    float num2=-mass2*gravity*sin(a1-2*a2);
    float num3=-2*sin(a1-a2)*mass2;
    float num4 = a2_v*a2_v*r2 + a1_v * a1_v*r1*cos(a1-a2);

    float den = r1*(2*mass1+mass2-mass2*cos(2*a1-2*a2));

    a1_a = (num1 + num2+num3*num4)/den;

    num1= 2*sin(a1-a2);
    num2 = (a1_v*a1_v*r1* (mass1+mass2));
    num3 = gravity*(mass1+mass2)*cos(a1);
    num4= a2_v*a2_v*r2*mass2*cos(a1-a2);

    den = r2*(2*mass1+mass2-mass2*cos(2*a1-2*a2));

    a2_a=(num1*(num2+num3+num4) )/ den;
  }
}