#include <stdio.h>
#include <math.h>

int main()
{
  int i,j,k;
  float m1,m2,M,r,r1,r2,h,g;
  double  ath1,ath2,vth1,vth2,th1,th2;
  double  y1,y2,x11,x12,x21,x22,div;
  float t=0;
  float v1,v2,v,xx,yy,thb;
  float v1l;
  float thbo=-90;
  float add=0;  
  float PI=3.1415927;
  float dt=0.0001;
  float minv1,minth1,minvb,minthb;
  int count=0;
  int decrease=0;   
  int deg45=0;
  
  g=9.8;
  M=1.5;
  //  M=2.5;
  m1=0.010;
  m2=0.048;
  r=1.5*2.54/100.;
  r1=10.*2.54/100.;
  r2=10.*2.54/100.;
  h=0.58;

  printf(" enter M R m1 r1 m2 r2:(e.g. 1.5 1.5 0.01 10. 0.048 10)\n");
  scanf("%f %f %f %f %f %f",&M,&r,&m1,&r1,&m2,&r2);
  printf(" M=%f R=%f \n m1=%f r1=%f \n m2=%f r2=%f \n",M,r,m1,r1,m2,r2);
  r=r*2.54/100.;
  r1=r1*2.54/100.;
  r2=r2*2.54/100.;


  ath1=0;
  ath2=0;
  vth1=0.0;
  vth2=0.0;
  th1=-90./180.*PI;
  th2=-90./180.*PI;

  minv1=10000.;
  decrease=0;
 LOOP:
  count=count+1;
  /* calculate coefficents of two equations in two unknowns ath1 and ath2 */
 
    // oscillation
    /*    y1=-m2*g*r1*cos(th1)-m2*r1*r2*sin(th1-th2)*vth2*vth2;
    x11=-m1*r1*r1-m2*r1*r1-M*r*r;
    x12=-m2*r1*r2*cos(th1-th2);
    y2=-m2*g*r2*cos(th2)+m2*r1*r2*sin(th1-th2)*vth1*vth1;
    x21=-m2*r1*r2*cos(th1-th2);
    x22=-m2*r2*r2; */
    
  // trebuche
  x11=-m2*r1*r1-m1*r1*r1-M*r*r;
  x12=-m2*r1*r2*(sin(th1)*sin(th2)+cos(th1)*cos(th2));
  y1=M*g*h/2/PI/2.5-m2*g*r1*cos(th1)+m2*vth2*vth2*r1*r2*(cos(th1)*sin(th2)-sin(th1)*cos(th2));
  x21=-m2*r1*r2*(sin(th1)*sin(th2)+cos(th1)*cos(th2));
  x22=-m2*r2*r2;
  y2=-m2*r2*g*cos(th2)-vth1*vth1*m2*r1*r2*(cos(th1)*sin(th2)-sin(th1)*cos(th2));

  /* solve two equations in two unknowns */

    div=x11*x22-x21*x12;
    ath1=(x12*y2-x22*y1)/div;
    ath2=(x21*y1-x11*y2)/div;

    t=t+dt;
    th1=th1+vth1*dt;
    th2=th2+vth2*dt;
    vth1=vth1+ath1*dt;
    vth2=vth2+ath2*dt;

    /* now we have new variable calculate interesting quantities */
    xx=r1*cos(th1)+r2*cos(th2);
    yy=r1*sin(th1)+r2*sin(th2);
    thb=atan2(yy,xx)*180/PI;
    
    thbo=thb;
    if(th1*180/PI-thb>250){
      j=th1*180/PI-thb+100;
      k=j/360;
      thb=thb+k*360;
    }

    v=sqrt(r1*r1*vth1*vth1+r2*r2*vth2*vth2+2*r1*r2*cos(th1-th2)*vth1*vth2);
    v1l=v1;
    v1=r1*vth1;
    v2=r2*vth2;
    if(v1<v1l)decrease=1;
    if(th1*180./PI>30.0&&v1<minv1){
       minv1=v1;
       minth1=th1;
       minthb=thb; 
       minvb=v; 
    }
    if(th1*180/PI>30.0&&v1>minv1&&decrease==1){
       printf(" Magic Point: %f th1=%f thb=%f vb=%f \n",minv1,minth1*180/PI,minthb,minvb);
       minv1=10000.;
       decrease=0;
    }
    if(deg45==0&&thb>765.){
       printf(" 45 degrees: th1=%f thb=%f vb=%f \n",th1*180/PI,thb,v);
       deg45=1;
    }

    if(thb>900)goto ENDR;
    // if(count%10==0)printf("%d %f %f %f %f %f %f \n",count,t,th1*180./PI,thb,v1,v2,v);
    goto LOOP;

 ENDR:

    return 1;
}