#define PI 3.14159265358979323846264338327950288419716939937510582097494459230781640628620899862803482534211706798214808651328230664709384460955058223172535940812848111745028410270193852110555964 // I know all about certian floating-point limitations...

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <X11/Xlib.h>
#include <X11/Xproto.h>
#include <X11/Xutil.h>
#include <X11/keysym.h>

#include <X11/Intrinsic.h>
#include <Xm/Xm.h>
#include <Xm/PushB.h>
#include <Xm/DrawingA.h>
#include <Xm/Form.h>
#include <Xm/Label.h>
#include <Xm/TextFP.h>
#include <Xm/TextP.h>
#include <Xm/Text.h>
#include <Xm/ToggleB.h>

#include <math.h>

#define WG_FORM          0
#define WG_DRAW          1
#define WG_INFOFORM      2
#define WG_QUIT          3
#define WG_START         4
#define WG_STOP          5
#define WG_START_STEP    6
#define WG_DO_STEP       7
#define WG_INFORMATION   8

#define NUM_WIDGETS 9

#define STRING_LENGTH    0
#define ANGULAR_SPEED    1
#define AMPLITUDE        2
#define PHASE_ANGLE      3
#define START_ANGLE      4
#define MASS             5
#define RADIUS           6
#define LOCAL_G          7
#define VISCOUS_DAMPING  8
#define VISC_ROT_DAMPING 9
#define MIN_TIMESTEP     10
#define MAX_TIMESTEP     11

#define NUM_USER_DEF_DBL 12

#define NUM_DBL          12

#define SOLID_STRING     0
#define NO_WIND_UP       1

#define NUM_USER_DEF_INT 2

#define NUM_INT          2

#define INTERVAL         25 // milliseconds

#define DIS_NONE          0
#define DIS_ROOF          1
#define DIS_STRING_BROKEN 2

int catastrophe; 

#define __min(a,b) ((a)<(b)?(a):(b))
#define sqr(a) ((a)*(a))

typedef struct
{
  char cLabel[30];
  char cStart[30];
  char cUnit[30];
  double min;
  double max;
  int dblLocator;
  char help[256];
  Widget label;
  Widget input;
  Widget unit;
} inputDataSystem;

char cInfoText[1024];

inputDataSystem InDataSys[NUM_USER_DEF_DBL] = {
  { "Snörlängd"           ,"2"          ,"m"     ,  0.0, 10.0,STRING_LENGTH,
    "Ange snörets längd. Detta är längden av det fria snöret då 'ryck'-anordningen är i mittenläge.&&Värden: 0 - 10 m"},
  { "'Ryck'-vinkelhast"   ,"1"          ,"1/s"   ,  0.0,100.0,ANGULAR_SPEED,
    "Ange hastigheten för förändringen hos snörets längd.&&Värden: 0 - 100 1/s"},
  { "'Ryck'-amplitud"     ,"1"          ,"m"     ,  0.0, 10.0,AMPLITUDE,
    "Ange amplituden för förändringen hos snörets längd.&&Värden: 0 - mindre är 'Snörlängd'"},
  { "'Ryck'-fasvinkel"    ,"0"          ,"grader",  0.0,360.0,PHASE_ANGLE,
    "Ange hur förändringen hos snörets längd skall starta.&&Värden: 0 - 360 grader"},
  { "Startvinkel"         ,"45"         ,"grader",-90.0, 90.0,START_ANGLE,
    "Ange var kulan skall starta i förhållande till lodlinjen.&&Värden -90 - 90 grader"},
  { "Föremåls massa"      ,"1"          ,"kg"    ,  0.0, 10.0,MASS,
    "Ange föremålets massa.&&Värden 0 - 10 kg"},
  { "Föremåls radie"      ,".5"         ,"m"     ,  0.0,  1.0,RADIUS,
    "Ange föremålets radie.&&Värden 0 - 1 m"},
  { "Lokalt g"            ,"9.82"       ,"m/s^2" ,  0.0,100.0,LOCAL_G,
    "Ange det lokala g.&&Värden 0 - 100 m/s^2"},
  { "Dämpning (mot v)"     ,"0"          ,"Ns/m"  ,  0.0,100.0,VISCOUS_DAMPING,
    "Ange dämpningen som är proportionell mot hastigheten.&&Värden 0 - 100 Ns/m"},
  { "Dämpning (mot w)"    ,"0"          ,"Ns"    ,  0.0,100.0,VISC_ROT_DAMPING,
    "Ange dämpningen som är proportionell mot föremålets rotationshastighet.&&Värden 0 - 100 Ns"},
  { "Största tidssteg"    ,"0.001"      ,"s"     ,0.0001,0.01,MAX_TIMESTEP,
    "Detta tidssteg används normalt vid beräkningarna.&&Värden 0.01 - 0.0001 Ns/m"},
  { "Minsta tidssteg"     ,"0.00001"    ,"s"     ,0.0000001,0.0001,MIN_TIMESTEP,
    "Om beräkningarna hamnar i ett läge då ökad noggrannhet önskas, används detta tidssteg.&&Värden 0.0001 - 0.0000001 Ns"}};

double dbl[NUM_DBL];

typedef struct
{
  char cLabel[30];
  int tglintLocator;
  char help[256];
  Widget input;
} inputToggleSystem;

inputToggleSystem InToggleSys[NUM_USER_DEF_INT] = {
  { "Oböjlig stång istf. snöre",SOLID_STRING,"Om en stång används istället för ett snöre kan den också trycka på kulan när den är på väg ner."},
  { "Rulla inte snöre runt kulan",NO_WIND_UP,"Snöret rullar inte upp sig runt kulan utan går rakt igenom. Något overkligt..." }};
  
int tglint[NUM_DBL];

void init_simulation ();
void do_calculation (long milliseconds);

XmString xm_String(char *s);
GC makeGC(Display *display, unsigned int fg, unsigned int bg);

/* Datastruktur som används vid omritning av fönstret */
typedef struct
{
  XtAppContext appContext;
  Display *    display;
  Widget       shell;
  GC           blackGC;
  GC           whiteGC;
  XtIntervalId idTimer;
  int          bTimer;
  Widget*      drawWidget;
} GfxData;

Widget*      infoWidget;

/* Kommer att anropas när fönstret behöver ritas om */
void exposeCB(Widget widget,
              XtPointer clientData, 
              XtPointer);

#define MAX_HELP_LEN 40

void displayHelp(char* errorString)
{
  for (;;)
    {
      char *ptr2;
      char *ptr;

      ptr = strrchr (errorString,'\n');
      if (ptr == NULL)
	ptr = errorString;
      ptr2 = strstr (ptr,"&&");
      if (ptr2 != NULL && ptr2-ptr < MAX_HELP_LEN)
	{
	  *ptr2 = '\n';
	  *(ptr2+1) = '\n';
	  continue;
	}
      if (strlen (ptr) < MAX_HELP_LEN)
	break;
      ptr2 = ptr+MAX_HELP_LEN-1;
      while (ptr2 != ptr && *ptr2 != ' ')
	ptr2--;
      if (ptr2 == ptr)
	{
	  ptr2 = strchr (ptr,' ');
	  if (ptr2 == NULL)
	    break;
	}
      *ptr2 = '\n';
    }
  
  XmTextSetString (*infoWidget,errorString);
}

void displayHelpCB(Widget widget,
		   XtPointer clientData,
		   XtPointer foo)
{
  char errorString[1024];
  strcpy (errorString,(char*) clientData);

  displayHelp (errorString);
}

/* Kommer att anropas när sluta-knappen släpps upp */
void quitCB(Widget widget,
	    XtPointer clientData, 
	    XtPointer foo)
{
  GfxData *data = (GfxData *)clientData;

  if (data->bTimer)
    {
      XtRemoveTimeOut (data->idTimer);
      data->bTimer = FALSE;
    }
  exit(0);
}

void timerCB(XtPointer clientData, 
	    XtIntervalId* foo)
{
  GfxData *data = (GfxData *)clientData;

  do_calculation (INTERVAL);

  exposeCB(*(data->drawWidget),data,0); // I'm not sure about the last argument, but as it isn't used...

  data->idTimer = XtAppAddTimeOut (data->appContext,INTERVAL,timerCB,(XtPointer) data);

}

int not_ok = TRUE;

void dostepCB(Widget widget,
	      XtPointer clientData, 
	      XtPointer xp)
{
  GfxData *data = (GfxData *)clientData;

  if (not_ok)
    return;

  do_calculation (10);

  exposeCB(*(data->drawWidget),data,0); // I'm not sure about the last argument, but as it isn't used...
}

void startstepCB(Widget widget,
		 XtPointer clientData, 
		 XtPointer foo)
{
  GfxData *data = (GfxData *)clientData;
  char* tmp, *endptr;
  int i;

  if (data->bTimer)
    {
      XtRemoveTimeOut (data->idTimer);
      data->bTimer = FALSE;
    }

  for (i = 0; i < sizeof (InDataSys)/sizeof(inputDataSystem); i++)
    {
      tmp = XmTextFieldGetString(InDataSys[i].input);
      dbl[InDataSys[i].dblLocator] = strtod (tmp,&endptr);
      if (endptr == tmp || dbl[InDataSys[i].dblLocator] < InDataSys[i].min || dbl[InDataSys[i].dblLocator] > InDataSys[i].max)
	{
	  char errorString[256];
	  sprintf (errorString,"Ogiltig: '%s'. (%f - %f)",InDataSys[i].cLabel,InDataSys[i].min,InDataSys[i].max);
	  XmTextSetString (*infoWidget,errorString);
	  not_ok = TRUE;
	  return;
	}
    }
  
  for (i = 0; i < sizeof (InToggleSys)/sizeof(inputToggleSystem); i++)
    {
      tglint[InToggleSys[i].tglintLocator] = XmToggleButtonGetState(InToggleSys[i].input);
    }

  if (dbl[AMPLITUDE] >= dbl[STRING_LENGTH])
    {
      char str[1024];
      sprintf (str,"'Amplitud' får ej vara större eller lika med än 'snörlängd'.\n");
      XmTextSetString (*infoWidget,str);
      not_ok = TRUE;
      return;
    } 
  not_ok = FALSE;
    
  init_simulation ();

  exposeCB(*(data->drawWidget),data,0); // I'm not sure about the last argument, but as it isn't used...

//  data->idTimer = XtAppAddTimeOut (data->appContext,INTERVAL,timerCB,(XtPointer) data);
//  data->bTimer = TRUE;
}

void SolidStringChanged(Widget widget,
	     XtPointer clientData, 
	     XtPointer xp)
{
  if (XmToggleButtonGetState(InToggleSys[SOLID_STRING].input))
    XmToggleButtonSetState(InToggleSys[NO_WIND_UP].input,TRUE,TRUE);
}

void startCB(Widget widget,
	     XtPointer clientData, 
	     XtPointer xp)
{
  GfxData *data = (GfxData *)clientData;

  startstepCB (widget,clientData,xp);

  if (not_ok)
    return;

  data->idTimer = XtAppAddTimeOut (data->appContext,INTERVAL,timerCB,(XtPointer) data);
  data->bTimer = TRUE;
}

void stopCB(Widget widget,
	    XtPointer clientData, 
	    XtPointer foo)
{
  GfxData *data = (GfxData *)clientData;
  if (data->bTimer)
    {
      XtRemoveTimeOut (data->idTimer);
      data->bTimer = FALSE;
    }
}

/* Kommer att anropas vid musklick */
void buttonpressEH(Widget widget,
                   XtPointer clientData,
                   XEvent *event,
                   Boolean *cont)
{
  GfxData *data = (GfxData *)clientData;

//  printf("musklick vid %d, %d\n", 
//         event->xbutton.x, event->xbutton.y);
}

int main(int argc, char *argv[])
{
  GfxData gfxdata;
  unsigned int black;
  unsigned int white;
  int i;
  int x, y;
  Widget wg[NUM_WIDGETS];

  gfxdata.bTimer = FALSE;

  printf ("\nProgram av Håkan T. Johansson\n\n");
  printf ("Utdrag ur 'Open-Ended Problems and computer simulations\nin Newtonian mechanics' av Arne Kihlberg:\n\n");
  printf ("III 7. The length of a pendulum string may be varied\nperiodically. This can be done by pulling it through\na hole. Examine how the solutions are affected for\ndifferent frequency and amplitude of the variation.\n\n");
  /* Initiera Xt och skapa en shell-widget */ 
  gfxdata.shell = XtAppInitialize (&gfxdata.appContext, 
                                   "Minimal Motif", NULL, 0, 
                                   &argc, argv,
                                   NULL,  NULL, 0);

  /* Initiera data som behövs för att rita */

  gfxdata.display = XtDisplay(gfxdata.shell);

  black           = BlackPixel(gfxdata.display,
                               DefaultScreen(gfxdata.display));

  white           = WhitePixel(gfxdata.display,
                               DefaultScreen(gfxdata.display));

//  printf ("%d %d\n",black,white);

  gfxdata.blackGC = makeGC(gfxdata.display, black, white);
  gfxdata.whiteGC = makeGC(gfxdata.display, white, white);

  /* Skapa en kompositwidget (XmForm) att lägga barnen i */
  wg[WG_FORM] = XtCreateWidget("form",xmFormWidgetClass,
			       gfxdata.shell,NULL, 0);

  wg[WG_INFOFORM] = XtCreateWidget("infoform",xmFormWidgetClass,
				   wg[WG_FORM],NULL, 0);
    
  /* Skapa en widget att rita i (XmDrawingArea) */
  wg[WG_DRAW] =  XtCreateWidget("draw",xmDrawingAreaWidgetClass,
                         wg[WG_FORM],NULL, 0);
  
  /* Skapa en widget för att avsluta (XmPushButton) */
  wg[WG_QUIT] =  XtCreateWidget("quitbutton",xmPushButtonWidgetClass,
			       wg[WG_INFOFORM],NULL, 0);
  wg[WG_START] =  XtCreateWidget("startbutton",xmPushButtonWidgetClass,
			       wg[WG_INFOFORM],NULL, 0);
  wg[WG_STOP] =  XtCreateWidget("stopbutton",xmPushButtonWidgetClass,
			       wg[WG_INFOFORM],NULL, 0);
  wg[WG_START_STEP] =  XtCreateWidget("startstepbutton",xmPushButtonWidgetClass,
			       wg[WG_INFOFORM],NULL, 0);
  wg[WG_DO_STEP] =  XtCreateWidget("dostepbutton",xmPushButtonWidgetClass,
			       wg[WG_INFOFORM],NULL, 0);
  wg[WG_INFORMATION] =  XtCreateWidget("information",xmTextWidgetClass,
				       wg[WG_INFOFORM],NULL, 0);

  for (i = 0; i < sizeof (InDataSys)/sizeof(inputDataSystem); i++)
    {
      InDataSys[i].label = XtCreateWidget("dummy",xmLabelWidgetClass,wg[WG_INFOFORM],NULL, 0);
      InDataSys[i].input = XtCreateWidget("dummy",xmTextFieldWidgetClass,wg[WG_INFOFORM],NULL, 0);
      InDataSys[i].unit = XtCreateWidget("dummy",xmLabelWidgetClass,wg[WG_INFOFORM],NULL, 0);
    }
  for (i = 0; i < sizeof (InToggleSys)/sizeof(inputToggleSystem); i++)
    {
      InToggleSys[i].input = XtCreateWidget("dummy",xmToggleButtonWidgetClass,wg[WG_INFOFORM],NULL, 0);
    }
  
  /* Fäst draw i form, utom nedtill */
  XtVaSetValues(wg[WG_INFOFORM],
                XmNleftAttachment,     XmATTACH_FORM,
                XmNrightAttachment,    XmATTACH_FORM,
                XmNtopAttachment,      XmATTACH_FORM,
                XmNwidth,              600,
                XmNheight,             30+30*((sizeof (InDataSys)/sizeof(inputDataSystem)+sizeof (InToggleSys)/sizeof(inputToggleSystem)+4)/2),
					      NULL);
		
  /* Fäst draw i form, utom nedtill */
  XtVaSetValues(wg[WG_DRAW],
                XmNleftAttachment,     XmATTACH_FORM,
                XmNrightAttachment,    XmATTACH_FORM,
                XmNtopAttachment,      XmATTACH_WIDGET,
		XmNtopWidget,          wg[WG_INFOFORM],
                XmNbottomAttachment,   XmATTACH_FORM,
                XmNwidth,              600,
                XmNheight,             500,
                NULL);

/* Fäst button i form, utom upptill där button sitter fast i draw */
  XtVaSetValues(wg[WG_QUIT],
                XmNleftAttachment,     XmATTACH_FORM,
                XmNlabelString,        xm_String("Avsluta"),
                XmNwidth,              70,
                XmNheight,             30,
                NULL);
  XtVaSetValues(wg[WG_START],
                XmNleftAttachment,     XmATTACH_WIDGET,
		XmNleftWidget,         wg[WG_QUIT],
                XmNlabelString,        xm_String("Starta simulering"),
                XmNwidth,              130,
                XmNheight,             30,
                NULL);
  XtVaSetValues(wg[WG_STOP],
                XmNleftAttachment,     XmATTACH_WIDGET,
		XmNleftWidget,         wg[WG_START],
                XmNlabelString,        xm_String("Stoppa simulering"),
                XmNwidth,              130,
                XmNheight,             30,
                NULL);
  XtVaSetValues(wg[WG_START_STEP],
                XmNleftAttachment,     XmATTACH_WIDGET,
		XmNleftWidget,         wg[WG_STOP],
		XmNlabelString,        xm_String("Initialisera stegning"),
                XmNwidth,              140,
                XmNheight,             30,
                NULL);
  XtVaSetValues(wg[WG_DO_STEP],
                XmNleftAttachment,     XmATTACH_WIDGET,
		XmNleftWidget,         wg[WG_START_STEP],
		XmNlabelString,        xm_String("Stega 0.01 sekund"),
                XmNwidth,              130,
                XmNheight,             30,
                NULL);

  x = 0;
  y = 0;

  for (i = 0; i < sizeof (InDataSys)/sizeof(inputDataSystem); i++)
    {
      XtVaSetValues(InDataSys[i].label,
		    XmNlabelString,        xm_String(InDataSys[i].cLabel),
		    XmNwidth,              150,
		    XmNheight,             30,
		    XmNx,                  0+x,
		    XmNy,                  30+y*30,
		    NULL);
      XtVaSetValues(InDataSys[i].input,
		    XmNwidth,              100,
		    XmNheight,             30,
		    XmNx,                  150+x,
		    XmNy,                  30+y*30,
		    NULL);
      XtVaSetValues(InDataSys[i].unit,
		    XmNlabelString,        xm_String(InDataSys[i].cUnit),
		    XmNwidth,              50,
		    XmNheight,             30,
		    XmNx,                  250+x,
		    XmNy,                  30+y*30,
		    NULL);
      XmTextFieldSetString(InDataSys[i].input,InDataSys[i].cStart);
      dbl[InDataSys[i].dblLocator] = strtod (InDataSys[i].cStart,NULL);

//      printf ("%d %d\n",x,y);

      y++;
      if (i == (sizeof (InDataSys)/sizeof(inputDataSystem)+sizeof (InToggleSys)/sizeof(inputToggleSystem)+2)/2)
	{
	  x += 300;
	  y = 0;
	}
    }

  for (i = 0; i < sizeof (InToggleSys)/sizeof(inputToggleSystem); i++)
    {
      XtVaSetValues(InToggleSys[i].input,
		    XmNlabelString,        xm_String(InToggleSys[i].cLabel),
		    XmNwidth,              240,
		    XmNheight,             30,
		    XmNx,                  30+x,
		    XmNy,                  30+y*30,
		    NULL);

//      XmTextFieldSetString(InToggleSys[i].input,InToggleSys[i].cStart);
      tglint[InToggleSys[i].tglintLocator] = 0;

//      printf ("%d %d\n",x,y);

      y++;
      if (i == (sizeof (InDataSys)/sizeof(inputDataSystem)+sizeof (InToggleSys)/sizeof(inputToggleSystem)+2)/2)
	{
	  x += 300;
	  y = 0;
	}
    }

  XtVaSetValues(wg[WG_INFORMATION],
                XmNwidth,              300,
                XmNheight,             (((sizeof (InDataSys)/sizeof(inputDataSystem)+sizeof (InToggleSys)/sizeof(inputToggleSystem)+4)/2)-y)*30,
		XmNx,                  x,
		XmNy,                  30+y*30,
                NULL);

  init_simulation ();

  /* Sätt upp callbacks för events */
  XtAddCallback (wg[WG_DRAW], XmNexposeCallback, exposeCB,
                 (XtPointer) &gfxdata);
  gfxdata.drawWidget = &(wg[WG_DRAW]);
  infoWidget = &(wg[WG_INFORMATION]);

  XtAddEventHandler (wg[WG_DRAW], ButtonPressMask, FALSE, buttonpressEH, 
                     (XtPointer) &gfxdata);

  XtAddCallback (wg[WG_QUIT], XmNactivateCallback, quitCB,
                 (XtPointer) &gfxdata);
  XtAddCallback (wg[WG_START], XmNactivateCallback, startCB,
                 (XtPointer) &gfxdata);
  XtAddCallback (wg[WG_STOP], XmNactivateCallback, stopCB,
                 (XtPointer) &gfxdata);
  XtAddCallback (wg[WG_START_STEP], XmNactivateCallback, startstepCB,
                 (XtPointer) &gfxdata);
  XtAddCallback (wg[WG_DO_STEP], XmNactivateCallback, dostepCB,
                 (XtPointer) &gfxdata);

  XtAddCallback (wg[WG_QUIT], XmNarmCallback, displayHelpCB,
                 (XtPointer) (char*) "Avslutar simulatorn.");
  XtAddCallback (wg[WG_START],XmNarmCallback,displayHelpCB,
		 (XtPointer) (char*) "Initialisera variabler för en simulering. Starta timern.");
  XtAddCallback (wg[WG_STOP],XmNarmCallback,displayHelpCB,
		 (XtPointer) (char*) "Avbryt simulering.");
  XtAddCallback (wg[WG_START_STEP],XmNarmCallback,displayHelpCB,
		 (XtPointer) (char*) "Initialisera variabler för en simulering.");
  XtAddCallback (wg[WG_DO_STEP],XmNarmCallback,displayHelpCB,
		 (XtPointer) (char*) "Stega simuleringen en liten tid.");

  for (i = 0; i < NUM_WIDGETS; i++)
    XtManageChild(wg[i]);
  for (i = 0; i < sizeof (InDataSys)/sizeof(inputDataSystem); i++)
    {
      XtManageChild(InDataSys[i].label);
      XtManageChild(InDataSys[i].input);
      XtManageChild(InDataSys[i].unit);
      XtAddCallback (InDataSys[i].input,XmNfocusCallback,displayHelpCB,
		     (XtPointer) InDataSys[i].help);
    }
  for (i = 0; i < sizeof (InToggleSys)/sizeof(inputToggleSystem); i++)
    {
      XtManageChild(InToggleSys[i].input);
      XtAddCallback (InToggleSys[i].input,XmNarmCallback,displayHelpCB,
		     (XtPointer) InToggleSys[i].help);
    }
  XtAddCallback (InToggleSys[SOLID_STRING].input,XmNvalueChangedCallback,SolidStringChanged,NULL);

  /* Visa fönstret */
  XtRealizeWidget(gfxdata.shell);

  /* Gå in in Xt's eventloop */
  XtAppMainLoop(gfxdata.appContext);
}


/* Skapa en XmString med eventuell radbrytningar ('\n') */
XmString xm_String(char *s)
{
    XmString xms1;
    XmString xms2;
    XmString line;
    XmString separator;
    char     *p;
    char     *t = (char*) XtNewString(s);   /* Make a copy for strtok not to */
                                    /* damage the original string    */

    separator = XmStringSeparatorCreate();
    p         = strtok(t,"\n");
    xms1      = XmStringCreateSimple(p);

    while (p = strtok(NULL,"\n"))
    {
        line = XmStringCreateSimple(p);
        xms2 = XmStringConcat(xms1,separator);
        XmStringFree(xms1);
        xms1 = XmStringConcat(xms2,line);
        XmStringFree(xms2);
        XmStringFree(line);
    }

    XmStringFree(separator);
    XtFree(t);
    return xms1;
}

/* Skapa en GC med förgrundsfärg 'fg' och bakgrundsfärg 'bg' */
GC makeGC(Display *display, unsigned int fg, unsigned int bg)
{
  XGCValues gcv;
  GC gc;

  gcv.foreground = fg;
  gcv.background = bg;
  gcv.function   = GXcopy;

  gc = XCreateGC(display,
                 DefaultRootWindow(display),
                 GCForeground | GCBackground | GCFunction,
                 &gcv);
  return gc;
  
}

double string_length;
double string_length_now;
double angular_speed;
double amplitude;
double phase_angle;
//double angle;
double mass;
double radius;
double local_g;
double damping;
double rotdamping;
// double xpos, ypos, xvel, yvel, ang_rot, ang_rot_speed;
// double time_now;
long calculations;
double timestep;
double mintimestep, maxtimestep;
double periodtime, lasttime;
int wascaught;
int xside;

#define XPOS            0
#define YPOS            1
#define ANG_ROT         2
#define XVEL            3
#define YVEL            4
#define ANG_ROT_SPEED   5
#define TIME_NOW        6

#define NUM_DBL_POS     7

double dblpos[3][NUM_DBL_POS];

#define VIRTUAL_SPRING_CONSTANT 100000 // N/m

double str_len_by_time (double time)
{
  double str_len = string_length+amplitude*(sin(angular_speed*time+phase_angle*PI/180.0));
  double theta, l;
  double a, alfa;

  if (tglint[NO_WIND_UP])
    return str_len;

  theta = atan2 (dblpos[0][YPOS],dblpos[0][XPOS]);
  l = hypot (dblpos[0][YPOS],dblpos[0][XPOS]);

  if (radius/l > 1)
    {
//      printf ("This shouldn't occur! - Model breakdown! - Unpredictable results may come!");
      return str_len;
    }

  a = asin (radius/l);

  alfa = dblpos[0][ANG_ROT];

  
  if (alfa > PI-theta-a)
    {
//      printf ("a alfa: %f theta: %f a: %f - %f\n",alfa,theta,a,radius*(alfa-(PI-theta-a)));
      str_len -= radius*(alfa-(PI-theta-a));
    }
  else
    {
      if (theta < 0)
	theta += 2*PI;
      if (alfa < a-theta)
	{
//	  printf ("b alfa: %f theta: %f a: %f - %f\n",alfa,theta,a,radius*(a-theta-alfa));
	  str_len -= radius*(a-theta-alfa);
	}
    }
  return str_len;
}

void getconnection (double* xcon,double* ycon,int* nBeginAng,int* nEndAng,
		    double* alf,
		    double* xncon,double* yncon,double onepixelis)
{
  double theta = atan2 (dblpos[0][YPOS],dblpos[0][XPOS]);
  double l = hypot (dblpos[0][YPOS],dblpos[0][XPOS]);
  double a, alfa, beginAng, endAng;

  if (radius/l > 1)
    {
      printf ("This shouldn't occur! - Model breakdown! - Unpredictable results may come!");
      return;
    }

  a = asin (radius/l);

  alfa = dblpos[0][ANG_ROT];

  beginAng = alfa+PI/2;
  endAng = alfa+PI/2;

  
  if (!tglint[NO_WIND_UP])
    {
      if (alfa > PI-theta-a)
	{
	  alfa = PI-theta-a;
	  beginAng = alfa+PI/2;
	}
      else
	{
	  if (theta < 0)
	    theta += 2*PI;
	  if (alfa < a-theta)
	    {
	      alfa = a-theta;
	      endAng = alfa+PI/2;
	    }
	}
    }
  if (nBeginAng != NULL)
    {
      *nBeginAng = (int) ((beginAng)*180*64/PI);
      *nEndAng = (int) ((endAng-beginAng > 2.01*PI ? 2.01*PI : endAng-beginAng)*180*64/PI)+1;
    }
  if (alf != NULL)
    *alf = alfa;

  if (xcon != NULL)
    {
      *xcon = dblpos[0][XPOS]-radius*sin(alfa);
      *ycon = dblpos[0][YPOS]-radius*cos(alfa);
    }
  if (xncon != NULL)
    {
      *xncon = dblpos[0][XPOS]-(radius+onepixelis)*sin(alfa);
      *yncon = dblpos[0][YPOS]-(radius+onepixelis)*cos(alfa);
    }

  /*
  double theta = atan2 (dblpos[0][YPOS],dblpos[0][YPOS]);

  double l = hypot (dblpos[0][YPOS],dblpos[0][YPOS]);

  double alfa = dblpos[0][ANG_ROT];
    
  alfa = fmod (alfa,2*PI);    

  double gamma = PI/2-alfa-theta;

  double k = sqrt (sqr(l)+sqr(radius)-2*radius*l*cos (gamma);

  double beta = asin (l*sin(gamma)/k);
  
  *xcon = dblpos[0][XPOS]-radius*sin(dblpos[0][ANG_ROT]);
  *ycon = dblpos[0][YPOS]-radius*cos(dblpos[0][ANG_ROT]);

  if (beta > PI)
    {
      *xcon = dblpos[0][XPOS]-radius*sin(dblpos[0][ANG_ROT]);
      *ycon = dblpos[0][YPOS]-radius*cos(dblpos[0][ANG_ROT]);
    }
  if (dblpos[0][ANG_ROT] > PI)
    {

    }*/
}

double str_vel_by_time (double time)
{
  double str_len_vel = angular_speed*amplitude*(cos(angular_speed*time+phase_angle*PI/180.0));
  double theta, l, a, alfa;

  if (tglint[NO_WIND_UP])
    return str_len_vel;

  theta = atan2 (dblpos[0][YPOS],dblpos[0][XPOS]);
  l = hypot (dblpos[0][YPOS],dblpos[0][XPOS]);

  if (radius/l > 1)
    {
      printf ("This shouldn't occur! - Model breakdown! - Unpredictable results may come!");
      return str_len_vel;
    }

  a = asin (radius/l);

  alfa = dblpos[0][ANG_ROT];

  
  if (alfa > PI-theta-a)
    {
//      printf ("a alfa: %f theta: %f a: %f - %f\n",alfa,theta,a,radius*(alfa-(PI-theta-a)));
      str_len_vel += radius*(dblpos[0][ANG_ROT_SPEED]);
    }
  else
    {
      if (theta < 0)
	theta += 2*PI;
      if (alfa < a-theta)
	{
//	  printf ("b alfa: %f theta: %f a: %f - %f\n",alfa,theta,a,radius*(a-theta-alfa));
	  str_len_vel -= radius*(dblpos[0][ANG_ROT_SPEED]);
	}
    }
  return str_len_vel;

  /*  return angular_speed*amplitude*(cos(angular_speed*time+phase_angle*PI/180.0)); */
}

double moment_of_inertia ()
{
  return 2.0/5.0*mass*sqr (radius);
}

void init_simulation ()
{
  string_length = dbl[STRING_LENGTH];
  angular_speed = dbl[ANGULAR_SPEED];
  amplitude = dbl[AMPLITUDE];
  phase_angle = dbl[PHASE_ANGLE];
  mass = dbl[MASS];
  radius = dbl[RADIUS];
  local_g = dbl[LOCAL_G];
  damping = dbl[VISCOUS_DAMPING];
  rotdamping = dbl[VISC_ROT_DAMPING];

  dblpos[0][TIME_NOW] = 0.0;

  dblpos[0][XVEL] = dblpos[0][YVEL] = dblpos[0][ANG_ROT_SPEED] = 0.0;
 
  dblpos[0][ANG_ROT] = dbl[START_ANGLE]*PI/180;
  string_length_now = string_length+amplitude*(sin(phase_angle*PI/180.0));
  for (;;)  
    { 
      dblpos[0][XPOS] = (string_length_now+radius) * sin (dblpos[0][ANG_ROT]);
      dblpos[0][YPOS] = (string_length_now+radius) * cos (dblpos[0][ANG_ROT]);
      if (dblpos[0][YPOS] < radius)
	{
	  dblpos[0][ANG_ROT] += (dblpos[0][ANG_ROT] > 0 ? -0.01 : 0.01);

	}
      else
	break;
    }
  

  calculations = 0;
  wascaught = TRUE;
  timestep = 0.00001;
  periodtime = 0.0;
  lasttime = 0.0;

  mintimestep = dbl[MIN_TIMESTEP];
  maxtimestep = dbl[MAX_TIMESTEP];

  catastrophe = DIS_NONE;

  xside = dblpos[0][XPOS] > 0.0;
}

void do_time_step (double tstep,double string_force)
{
  int i;

  double xforce = 0.0;
  double yforce = 0.0;

  double moment = 0.0;

  double xconnection, yconnection, alfa, length_to_connection, obj_to_far_away;
  double direction;

  yforce = mass*local_g;

  xforce -= damping*dblpos[0][XVEL];
  yforce -= damping*dblpos[0][YVEL];

  moment -= rotdamping*dblpos[0][ANG_ROT_SPEED];

  xconnection = dblpos[0][XPOS]-radius*sin(dblpos[0][ANG_ROT]);
  yconnection = dblpos[0][YPOS]-radius*cos(dblpos[0][ANG_ROT]);

  alfa;

  getconnection (&xconnection,&yconnection,
		 NULL,NULL,&alfa,NULL,NULL,0);

  length_to_connection = hypot (xconnection,yconnection);
  obj_to_far_away = length_to_connection - string_length_now;

  /*
  if (obj_to_far_away > 0)
    {
      double force = obj_to_far_away*VIRTUAL_SPRING_CONSTANT;
  
      double direction = atan2 (-yconnection,-xconnection);
      xforce += force*cos(direction);
      yforce += force*sin(direction);

      moment += force*radius*sin (direction-(PI/2-dblpos[0][ANG_ROT]));
    }
*/
  if (tglint[SOLID_STRING] || (obj_to_far_away > 0 && string_force == 0.0))
    string_force = obj_to_far_away*VIRTUAL_SPRING_CONSTANT;
  
  direction = atan2 (-yconnection,-xconnection);
  xforce += string_force*cos(direction);
  yforce += string_force*sin(direction);
  
  moment += string_force*radius*sin (direction-(PI/2-alfa));
//  moment += string_force*radius*sin (direction-(PI/2-alfa));
  


//  force*time=(veldif)*mass;
//  moment*time=(angveldif)*mom_of_inertia;

  dblpos[0][ANG_ROT_SPEED] += moment*tstep/moment_of_inertia ();

  dblpos[0][XVEL] += xforce*tstep/mass;
  dblpos[0][YVEL] += yforce*tstep/mass;

  for (i = 0; i < 3; i++)
    dblpos[0][i] += dblpos[0][i+3]*tstep;

  if (dblpos[0][YPOS] < radius)
    {
//      catastrophe = DIS_ROOF;

      dblpos[0][YPOS] = -0.6*(dblpos[0][YPOS]-radius)+radius;
      dblpos[0][YVEL] = -0.6*dblpos[0][YVEL];
    }

//  dblpos[0][XPOS] += dblpos[0][XVEL]*tstep;
//  dblpos[0][YPOS] += dblpos[0][YVEL]*tstep;
//  dblpos[0][ANG_ROT] += dblpos[0][ANG_ROT_SPEED]*tstep;
}

//#define MIN_TIME_STEP 0.00001
//#define MAX_TIME_STEP 0.001

void do_calculation (long milliseconds)
{
  double runtotime = dblpos[0][TIME_NOW] + milliseconds / 1000.0;
  int hasbeencaught;
  double string_force;

  double xconnection, yconnection;
  double alfa;
  double length_to_connection, obj_to_far_away;

  double direction;
  double theta;
  double gifh;

  for (; dblpos[0][TIME_NOW] < runtotime; )
    {
      memcpy (dblpos[1],dblpos[0],sizeof (dblpos[0]));

      string_length_now = str_len_by_time (dblpos[0][TIME_NOW]);

      timestep *= 1.01;
      if (timestep > maxtimestep)
	timestep = maxtimestep;
      /*      timestep = __min (timestep,0.0001); */

      hasbeencaught;
      string_force = 0.0;

      for ( ; ; )
      {
	do_time_step (timestep,string_force);
	if (catastrophe)
	  {
	    memcpy (dblpos[0],dblpos[1],sizeof (dblpos[0]));
	    break;
	  }

	xconnection = dblpos[0][XPOS]-radius*sin(dblpos[0][ANG_ROT]);
	yconnection = dblpos[0][YPOS]-radius*cos(dblpos[0][ANG_ROT]);
	
	getconnection (&xconnection,&yconnection,
		       NULL,NULL,&alfa,NULL,NULL,0);

	length_to_connection = hypot (xconnection,yconnection);
	obj_to_far_away = length_to_connection - str_len_by_time (dblpos[0][TIME_NOW]);
	
	if (!tglint[SOLID_STRING] && ((hasbeencaught = (obj_to_far_away > 0)) && !wascaught))
	  {
	    timestep /= 2.0;

	    if (timestep > mintimestep)
	      memcpy (dblpos[0],dblpos[1],sizeof (dblpos[0]));
	    else
	      {
		timestep = mintimestep;
		wascaught = TRUE;

		direction = atan2 (-yconnection,-xconnection);
		theta = atan2 (yconnection,xconnection);

		/*double gifh = sin(theta)*sin(dblpos[0][ANG_ROT])-cos(theta)*cos(dblpos[0][ANG_ROT]);*/
		gifh = sin(theta)*sin(alfa)-cos(theta)*cos(alfa);
		
		string_force = (str_vel_by_time (dblpos[0][TIME_NOW])-
				dblpos[0][XVEL]*cos(theta)-
				dblpos[0][YVEL]*sin(theta)-
				dblpos[0][ANG_ROT_SPEED]*radius*(gifh))/
		  (timestep*(-1/mass+
			     radius*radius*
			     sin(direction-(PI/2-alfa))/
			     moment_of_inertia ()*
			     (gifh)));
		/*
		string_force = (str_vel_by_time (dblpos[0][TIME_NOW])-
				dblpos[0][XVEL]*cos(theta)-
				dblpos[0][YVEL]*sin(theta)-
				dblpos[0][ANG_ROT_SPEED]*radius*(gifh))/
		  (timestep*(-1/mass+
			     radius*radius*
			     sin(direction-(PI/2-dblpos[0][ANG_ROT]))/
			     moment_of_inertia ()*
			     (gifh)));
			     */
		/*		printf ("Nice-catch\n"); */
	      }	    
	  }
	else if (xside != (dblpos[0][XPOS] > 0.0) && timestep > mintimestep * 2.0)
	  timestep /= 2.0;
	else
	  break;
	//		printf ("%.20f\n",timestep); 
      }
      if (catastrophe)
	break;
      dblpos[0][TIME_NOW] += timestep;

      wascaught = hasbeencaught;
      if (xside != (dblpos[0][XPOS] > 0.0))
	{
	  if (lasttime != 0.0)
	    periodtime = dblpos[0][TIME_NOW]-lasttime;
	  lasttime = dblpos[0][TIME_NOW];	
	  xside = dblpos[0][XPOS] > 0.0;
	}
      calculations++;
					  
    }
  sprintf (cInfoText,"Beräkningar: %d\nTid: %.3f\n",calculations,dblpos[0][TIME_NOW]);
  if (periodtime != 0.0)
    sprintf (cInfoText+strlen (cInfoText),"Periodtid: %.3f\n",periodtime*2.0);
  switch (catastrophe)
    {
    case DIS_ROOF:
      strcat (cInfoText,"\nFöremålet rörde taket.\n");
      break;
    case DIS_STRING_BROKEN:
      strcat (cInfoText,"\nSnöret gick av,\nbeklagar!\n");
      break;
    }
}

#define NUMLINESEGMENTS 20

void exposeCB(Widget widget,
              XtPointer clientData, 
              XtPointer foo)
{
  GfxData *data = (GfxData *)clientData;
  double xmax, ymax;
  double scalex, scaley;
  double scale;
  int addx, addy;
  int separatorY;
  double stringEndX, stringEndY;

  double xconnection, yconnection;

  int nBeginConnAng, nEndConnAng;

  double xp[NUMLINESEGMENTS+1];
  double yp[NUMLINESEGMENTS+1];
  double desiredlength, length, add, factor;
  int i, j, count;
  XWindowAttributes wnd_attrib;
  
  XClearWindow(data->display, XtWindow(widget));

//  memset (cInfoText+strlen (cInfoText),' ',sizeof (cInfoText)-strlen (cInfoText)-1);
//  cInfoText[sizeof (cInfoText)-1] = '\0';

  XmTextSetString (*infoWidget,cInfoText);


//  XDrawLine(data->display, XtWindow(widget), data->blackGC, 0, 0, 200, 200);

  XGetWindowAttributes (data->display, XtWindow(widget),
			&wnd_attrib);
//  printf ("%d,%d\n",wnd_attrib.width,wnd_attrib.height);

  if (wnd_attrib.width < 20 || wnd_attrib.height < 20)
    {
      XDrawLine(data->display, XtWindow(widget), data->blackGC, 
		0, 0, wnd_attrib.width, wnd_attrib.height);
      XDrawLine(data->display, XtWindow(widget), data->blackGC, 
		wnd_attrib.width, 0, 0, wnd_attrib.height);
      
      return;
    }
  xmax = (string_length+amplitude+radius*2)*2;
  ymax = string_length+amplitude+radius*2+amplitude*2;

  scalex = (wnd_attrib.width-10)/xmax;
  scaley = (wnd_attrib.height-10)/ymax;

  scale = __min (scalex,scaley);

  addx = (int) (wnd_attrib.width-xmax*scale)/2;
  addy = (int) (wnd_attrib.height-ymax*scale)/2;

  separatorY = (int) (scale*(amplitude*2));
  
//  printf ("---------------------------\n%f\n%f\n%f\n%f\n%f\n\n%d\n%d\n%d\n",
//	  xmax,ymax,scalex,scaley,scale,addx,addy,separatorY);

  XDrawLine(data->display, XtWindow(widget), data->blackGC, 
	    addx,addy+separatorY,addx+(int) (xmax*scale),addy+separatorY);
  XDrawRectangle(data->display, XtWindow(widget), data->blackGC, 
		 addx-3,addy-3,(int) (xmax*scale)+6,(int) (ymax*scale)+6);

  XDrawArc (data->display, XtWindow(widget), data->whiteGC,
	    addx+(int) (scale*(dblpos[0][XPOS]+xmax/2-radius)),
	    addy+separatorY+(int) (scale*(dblpos[0][YPOS]-radius)),
	    (int) (scale*radius*2),(int) (scale*radius*2),
	    0,360*64);

  XDrawLine(data->display, XtWindow(widget), data->blackGC, 
	    addx+(int)(scale*(xmax/2)),
	    addy+separatorY-
	    (int) (scale*(amplitude*(1-sin(angular_speed*dblpos[0][TIME_NOW]+phase_angle*PI/180.0)))),
	    addx+(int)(scale*(xmax/2)),
	    addy+separatorY);

  stringEndX = 0;
  stringEndY = string_length_now;

  xconnection = dblpos[0][XPOS]-radius*sin(dblpos[0][ANG_ROT]);
  yconnection = dblpos[0][YPOS]-radius*cos(dblpos[0][ANG_ROT]);

  getconnection (NULL,NULL,
		 &nBeginConnAng,&nEndConnAng,NULL,
		 &xconnection,&yconnection,1/scale);
  /*
  XDrawLine(data->display, XtWindow(widget), data->whiteGC,
	    addx+(int)            (scale*(dblpos[0][XPOS]+xmax/2)),
	    addy+separatorY+(int) (scale*(dblpos[0][YPOS]       )),
	    addx+(int)            (scale*(xconnection+xmax/2)),
	    addy+separatorY+(int) (scale*(yconnection       )));
	    */
  XDrawLine(data->display, XtWindow(widget), data->whiteGC,
	    addx+(int)            (scale*(dblpos[0][XPOS]+xmax/2)),
	    addy+separatorY+(int) (scale*(dblpos[0][YPOS]       )),
	    addx+(int)            (scale*(dblpos[0][XPOS]+xmax/2+0.8*radius*cos(PI/2-dblpos[0][ANG_ROT]))),
	    addy+separatorY+(int) (scale*(dblpos[0][YPOS]       +0.8*radius*sin(PI/2-dblpos[0][ANG_ROT]))));
  /*
  if (hypot (xconnection,yconnection) > 0)
    {
      double part = string_length_now / hypot (xconnection,yconnection);

      stringEndX = xconnection * part;
      stringEndY = yconnection * part;
    }
    */
  /*
  XDrawLine(data->display, XtWindow(widget), data->blackGC,
	    addx+(int)(scale*(xmax/2)),
	    addy+separatorY,
	    addx+(int) (scale*(stringEndX+xmax/2)),
	    addy+separatorY+(int) (scale*stringEndY));
	    */
  
  XDrawArc (data->display, XtWindow(widget), data->blackGC,
	    addx+(int) (scale*(dblpos[0][XPOS]+xmax/2-radius))-1,
	    addy+separatorY+(int) (scale*(dblpos[0][YPOS]-radius))-1,
	    (int) (scale*radius*2)+2,(int) (scale*radius*2)+2,
	    nBeginConnAng,nEndConnAng);
  if (wascaught)
    XDrawLine(data->display, XtWindow(widget), data->blackGC,
	      addx+(int)(scale*(xmax/2)),
	      addy+separatorY,
	      addx+(int) (scale*(xconnection+xmax/2)),
	      addy+separatorY+(int) (scale*yconnection));
  else
    {
      XPoint vertices[NUMLINESEGMENTS+1];
      vertices[0].x = addx+(int)(scale*(xmax/2));
      vertices[0].y = addy+separatorY;
      vertices[NUMLINESEGMENTS].x = addx+(int) (scale*(xconnection+xmax/2));
      vertices[NUMLINESEGMENTS].y = addy+separatorY+(int) (scale*yconnection);
      
      desiredlength = str_len_by_time (dblpos[0][TIME_NOW]);
      
      for (i = 0; i < NUMLINESEGMENTS+1; i++)
	{
	  xp[i] = xconnection*i/NUMLINESEGMENTS;
	  yp[i] = yconnection*i/NUMLINESEGMENTS;
	}
      for (count = 0; count < 10; count++)
	{
	  length = 0.0;
	  for (i = 0; i < NUMLINESEGMENTS; i++)
	    length += hypot (xp[i]-xp[i+1],yp[i]-yp[i+1]);
	  if (length / desiredlength > 0.97)
	    break;
	  add = (desiredlength - length) / (NUMLINESEGMENTS * cosh (NUMLINESEGMENTS / 2));
       
	  for (j = 1; j <= NUMLINESEGMENTS / 2; j++)
	    {
	      factor = add*(cosh (NUMLINESEGMENTS / 2) - cosh (NUMLINESEGMENTS / 2-j));
	      
	      if (tglint[NO_WIND_UP] || hypot (yp[j] + factor-dblpos[0][YPOS],
					       xp[j] -dblpos[0][XPOS]) > radius)
		yp[j] += factor;
	      if (j != NUMLINESEGMENTS - j)
		if (tglint[NO_WIND_UP] || hypot (yp[NUMLINESEGMENTS-j] + factor-dblpos[0][YPOS],
						 xp[NUMLINESEGMENTS-j] - dblpos[0][XPOS]) > radius)
		  yp[NUMLINESEGMENTS-j] += factor;
	    }
	}
      
      for (i = 1; i < NUMLINESEGMENTS; i++)
	{
	  vertices[i].x = addx+(int) (scale*(xp[i]+xmax/2));
	  vertices[i].y = addy+separatorY+(int) (scale*yp[i]);
	}
      
      XDrawLines (data->display, XtWindow(widget), data->blackGC,
		  vertices,NUMLINESEGMENTS+1,CoordModeOrigin);
      
    }

	        
  /*
  XDrawLine(data->display, XtWindow(widget), data->blackGC,
	    addx+(int)            (scale*(dblpos[0][XPOS]+xmax/2-1.3*radius*cos(dblpos[0][ANG_ROT]))),
	    addy+separatorY+(int) (scale*(dblpos[0][YPOS]       +1.3*radius*sin(dblpos[0][ANG_ROT]))),
	    addx+(int)            (scale*(dblpos[0][XPOS]+xmax/2-0.8*radius*cos(dblpos[0][ANG_ROT]))),
	    addy+separatorY+(int) (scale*(dblpos[0][YPOS]       +0.8*radius*sin(dblpos[0][ANG_ROT]))));
  XDrawLine(data->display, XtWindow(widget), data->blackGC,
	    addx+(int)            (scale*(dblpos[0][XPOS]+xmax/2+1.3*radius*cos(dblpos[0][ANG_ROT]))),
	    addy+separatorY+(int) (scale*(dblpos[0][YPOS]       -1.3*radius*sin(dblpos[0][ANG_ROT]))),
	    addx+(int)            (scale*(dblpos[0][XPOS]+xmax/2+0.8*radius*cos(dblpos[0][ANG_ROT]))),
	    addy+separatorY+(int) (scale*(dblpos[0][YPOS]       -0.8*radius*sin(dblpos[0][ANG_ROT]))));
	    */

}