#include "/usr/local/dislin/dislin.h"
#include <stdio.h>
#include <math.h>

double g = 9.81;
double l = 0.8;


double omega = 1.05;
double Omega = 0.0;



double phi = 0.0;
double phi_punkt = 0.0;
double energy = 0.0;

int step = 0;
int max_steps = 10000;

double h = 0.005;
double m_0 = 0.0;
double m_1 = 0.0;
double m_2 = 0.0;
double m_3 = 0.0;

int mouse_x = 0;
int mouse_y = 0;

double funct(double x,double y,double v) {
  //  return -(g/l)*sin(y);

  double r = 0.085;
  double m = 0.035;
  double J = 0.0137;
  double F = .08;
  double K = .02;
  double D = .0146;
  double N = m*g*r;

  return (F/J)*sin(omega * x + Omega) - K*v/J - D*y + N*sin(y)/J;
}

int sign(double num) {
  if (num >= 0.0) {
    return 1;
  } else {
    return -1;
  }
}

main() {

  double phi_punkt_alt = phi_punkt;
  double phi_min_alt = 0.0;
  double phi_max_alt = 0.0;

  FILE *fpmin = fopen("Pohl_min","w");
  FILE *fpmax = fopen("Pohl_max","w");


  metafl("CONS");  
  //x11mod("STORE");
  //  winmod("NOHOLD");  
  disini();
  
  for (;;) {

    axspos(250,1050);
    axslen(1000,1000);
    graf(-1.0,1.0,-1.0,0.5,-1.0,1.0,-1.0,0.5);
    endgrf();
    
    axspos(1600,1050);
    axslen(1000,1000);
    graf(-3.142,3.142,-3.0,1.0,-10.0,10.0,-10.0,2.0);
    
    csrpt1(&mouse_x, &mouse_y);
    
    if ( (mouse_x > 1600) && (mouse_x < 2600) && (mouse_y > 50) && mouse_y < 1050 ) {
      phi_punkt = (double)yinvrs(mouse_y);
      phi = (double)xinvrs(mouse_x); 
    } else {
      break;
    }
    endgrf();
    
    for ( step = 0; step < max_steps; step++ ) {
      
      //--{ Darstellung Ortsraum }--------------
      
      // Löschen des alten Bildes
      axspos(250,1050);
      axslen(1000,1000);
      graf(-1.0,1.0,-1.0,0.5,-1.0,1.0,-1.0,0.5);
      
      color("BLACK");
      rlvec(0, 0, l*sin(phi),-l*cos(phi),4221);
      hsymbl(50);
      rlsymb(21,l*sin(phi),-l*cos(phi));
      //---------------------------------------------------------------
      
      m_0 = h * funct(step * h, phi, phi_punkt);
      m_1 = h * funct((step + 0.5) * h, phi + 0.5 * h * phi_punkt, phi_punkt + 0.5 * m_0);
      m_2 = h * funct((step + 0.5) * h, phi + 0.5 * h * phi_punkt + 0.25 * h * m_0, phi_punkt + 0.5 *m_1);
      m_3 = h * funct((step + 1) * h, phi + h * phi_punkt + 0.5 * h * m_1, phi_punkt + m_2);
      

      phi_punkt_alt = phi_punkt;
      phi_punkt = phi_punkt + 1.0 / 6.0 * (m_0 +2.0*m_1 +2.0*m_2+m_3);

      phi = phi + h * phi_punkt + h / 6.0 * (m_0 + m_1 + m_2);

      //--{ Zeichnen der neuen Darstellung des Ortsraumes }---------------
      color("WHITE");
      rlvec(0, 0, l*sin(phi), -l*cos(phi),4221);
      hsymbl(35);
      rlsymb(21,l*sin(phi), -l*cos(phi));
      endgrf();
      //---------------------------------------------------------------
      
      axspos(1600,1050);
      graf(-3.142,3.142,-3.0,1.0,-10.0,10.0,-10.0,2.0);
      
      //      printf("Phi: %f\n",phi);
      hsymbl(5);

      if ( phi > - M_PI ) {
	rlsymb(21, -M_PI + fmod(phi + M_PI,2.0*M_PI), phi_punkt);
      } else {
	rlsymb(21, M_PI + fmod(phi + M_PI,2.0*M_PI), phi_punkt);
      }

      endgrf();

      //      axspos(250,2050);
      //axslen(1000,700);
      //graf(-3.0,3.0,-3.0,1.0,-3.0,3.0,-3.0,1.0);
      
      if ( (phi_punkt_alt > 0.0) && (phi_punkt < 0.0) ) {
	if (phi_min_alt) {
	  //  rlsymb(21,phi_min_alt,phi);
	  fpmin = fopen("Pohl_min","a");
	  fprintf(fpmin,"%f %f\n",phi_min_alt,phi);
	  fclose(fpmin);
	}
	phi_min_alt = phi;
      }
      //      endgrf();

      //      axspos(1600,2050);
      //axslen(1000,700);
      //graf(-3.0,3.0,-3.0,1.0,-3.0,3.0,-3.0,1.0);
      
      if ( (phi_punkt_alt < 0.0) && (phi_punkt > 0.0) ) {
	if (phi_max_alt) {
	  //  rlsymb(21,phi_max_alt,phi);
	  fpmax = fopen("Pohl_max","a");
	  fprintf(fpmax,"%f %f\n",phi_max_alt,phi);
	  fclose(fpmax);
	}
	phi_max_alt = phi;
      }
      //      endgrf();
     
      energy = 0.5 * pow(l,2) * pow(phi_punkt, 2) + g * l * (1.0 - cos(phi));
      //      printf("Energie: %f\n", energy);
      
      sendbf();
      usleep(3);       
    }
    
    axspos(250,1050);
    axslen(1000,1000);
    graf(-1.0,1.0,-1.0,0.5,-1.0,1.0,-1.0,0.5);
    
    color("BLACK");
    rlvec(0, 0, l*sin(phi),-l*cos(phi),4221);
    hsymbl(50);
    rlsymb(21,l*sin(phi),-l*cos(phi));
    color("WHITE");
    endgrf();

    axspos(1600,1050);
    graf(-3.142,3.142,-3.0,1.0,-10.0,10.0,-10.0,2.0);
    endgrf();
    sendbf();
  }  
  symfil("XWIN","DELETE");


  
  disfin();
}