#include <sys/types.h>
#include <unistd.h>
#include <fcntl.h>
#include <curses.h>
#include <math.h>
#include <signal.h>
#include "balance.h"

#define pi               M_PI
#define g                9.81

#define LOGFILE          "pendel.log"
#define m                1.0
#define l                1.0
#define deltaT           0.005
#define alpha0           25.0*(pi/180.0)
#define alphaDot0        0.0
#define a0               20.0

#define Bumper_alphaDot  0.5
#define Bumper_alpha     10.0*(pi/180.0)

#define Xcenter          30
#define Ycenter          23
#define Xscale           25.0
#define Yscale           20.0

double alpha,Fa,Fres,Fz,alphaDot,deltaAlpha,rho,Fg,beta;
FILE *Log;

/*************************************************************************/
void Delay(void)
{
    int flags;
    fcntl(0,F_GETFL,&flags);
    flags&=~O_NDELAY;
    fcntl(0,F_SETFL,flags);
}

void Nodelay(void)             { fcntl(0,F_SETFL,O_NDELAY); }
double sqr(double x)           { return x*x; }
void gotoxy(int x, int y)      { move(y-1,x); }
int round(double x)            { return (int)(x); }
double Max(double x, double y) { return (x>y)?x:y; }
double Min(double x, double y) { return (x<y)?x:y; }
double arccos(double x)        { return acos(x); }

/*************************************************************************/
void exitfn(int sig)
{
    gotoxy(1,25);
    printw("CR: ");
    refresh();

    nocbreak();
    echo();
    Delay();
    getch();

    endwin();
    if (Log) fclose(Log);

    exit(0);
}

/*************************************************************************/
void main(void)
{
    int ch;
    int cnt,n;
    int kx,ky;
    WINDOW *win;

    signal(SIGINT, exitfn);
    signal(SIGQUIT,exitfn);
    signal(SIGTERM,exitfn);
    
    if(!(Log=fopen(LOGFILE,"w"))){
	perror("Can't open log-file '" LOGFILE "'");
	exit (1);
    }
    
    win=initscr();
    cbreak();
    noecho();
    Nodelay();
    
    for(n=0;n<=180;n++){
	alpha=n*pi/180.0;
	kx=Xcenter+round(Xscale*cos(alpha));
	ky=Ycenter-round(Yscale*sin(alpha));
	 
	gotoxy(kx,ky);
	printw(".");
    }
    gotoxy(Xcenter,Ycenter);
    printw("+");
    refresh();

    balance_init();
    refresh();
    
    n        = 1;
    cnt      = 1;
    alphaDot = alphaDot0;
    alpha    = alpha0;
    Fa       = m*balance(alpha,alphaDot)*a0;
    Fg       = m*g;
    
    while (alpha>=0.0 && alpha<=pi){
	gotoxy(1,1);
	printw("%4d: ",n);
	printw("alpha = %7.3f ",(alpha*180.0/pi));
	printw("alphaDot = %6.3f ",alphaDot);
	printw("deltaAlpha = %6.3f ",deltaAlpha);
	printw("Fa = %6.3f ",Fa);
	printw("                                  ");
	fprintf(Log,"%d %6.3f\n",n,cos(alpha));
	
	gotoxy(kx,ky); addch('.');
	kx=Xcenter+round(Xscale*cos(alpha));
	ky=Ycenter-round(Yscale*sin(alpha));
	gotoxy(kx,ky); addch('@');
	refresh();

	/* Tastaturbehandlung */
	if((ch=getch())!=ERR){
	    switch(ch){
	      case 68:
		/* Bump left */
		alphaDot = alphaDot + Bumper_alphaDot;
		alpha    = alpha    + Bumper_alpha;
		break;
	      case 67:
		/* Bump right */
		alphaDot = alphaDot - Bumper_alphaDot;
		alpha    = alpha    - Bumper_alpha;
		break;
	      case 70:
		Delay();
		while(getch()==ERR);
		Nodelay();
		break;
	      case 'p':
		alpha=2*pi;
		break;
	    }
	}
	
	/* f2: Ausgleichen der Pendelbewegung */
	Fa = m*balance(alpha,alphaDot)*a0;

	/* f1: Pendel-Verhalten nach Ch. Ziegaus */
	Fres       = sqrt(sqr(Fg) + sqr(Fa));
	rho        = alpha - arccos(Fa/Fres);
	Fz         = sin(rho) * Fres;
	deltaAlpha = Fz/(2.0*m*l)*sqr(deltaT) + alphaDot*deltaT;
	alphaDot   = Fz/(m*l)*deltaT + alphaDot;
	alpha      = alpha + deltaAlpha;
	
	n          = n+1;
    }
    
    exitfn(0);
    /*NOTREACHED*/
}