program pendel;
uses crt;

const
     pi        = 3.141592654;
     g         = 9.81;

     m         = 1.0;
     l         = 1.0;
     deltaT    = 0.025;
     alpha0    = 60*(pi/180.0);
     alphaDot0 = 1.0;
     a0        = 500.0;
     a0_cheat  = 0.0;

     Bumper_alphaDot = 0.5;
     Bumper_alpha    = 10.0*(pi/180.0);

     Xcenter   = 30;
     Ycenter   = 23;
     Xscale    = 25.0;
     Yscale    = 20.0;

     NB        = 0;
     NM        = 1;
     NS        = 2;
     ZE        = 3;
     PS        = 4;
     PM        = 5;
     PB        = 6;

var
   alpha,
   Fa,
   Fres,
   Fz,
   alphaDot,
   deltaAlpha,
   gamma,
   Fg,
   beta:          real;
   n:             integer;
   c:             char;
   kx,ky:         integer;

var
   b_alpha_l: array[0..6] of real;
   b_alpha_u: array[0..6] of real;
   b_alphaDot_l: array[0..6] of real;
   b_alphaDot_u: array[0..6] of real;
   b_rule_alpha: array[0..6] of integer;
   b_rule_alphaDot: array[0..6] of integer;
   b_rule_a: array[0..6] of integer;
   b_bez: array[0..6] of string;

(*********************************************************************)
function arccos(x: real): real;
begin
     (* SEHR grobe Naeherung *)
     arccos := -0.5*pi*x + pi/2;
end;

(*********************************************************************)
function min(x,y: real): real;
begin
     if x<y then
        min:=x
     else
        min:=y;
end;

(*********************************************************************)
function maximum(x,y: real): real;
begin
     if x>y then
        maximum:=x
     else
        maximum:=y;
end;

(*********************************************************************)
procedure balance_init;
var
   i: integer;
begin
     (* Grenzen fuer Alpha *)
     b_alpha_l[0] := -30.0;   b_alpha_u[0] :=  30.0;
     b_alpha_l[1] :=   0.0;   b_alpha_u[1] :=  60.0;
     b_alpha_l[2] :=  30.0;   b_alpha_u[2] :=  90.0;
     b_alpha_l[3] :=  60.0;   b_alpha_u[3] := 120.0;
     b_alpha_l[4] :=  90.0;   b_alpha_u[4] := 150.0;
     b_alpha_l[5] := 120.0;   b_alpha_u[5] := 180.0;
     b_alpha_l[6] := 150.0;   b_alpha_u[6] := 210.0;

     for i:=0 to 6 do begin
         b_alpha_l[i]:=b_alpha_l[i]*pi/180.0;
         b_alpha_u[i]:=b_alpha_u[i]*pi/180.0;
     end;

     (* Grenzen fuer alphaDot *)
     b_alphaDot_l[0] := -5.0;   b_alphaDot_u[0] := -1.0;
     b_alphaDot_l[1] := -3.0;   b_alphaDot_u[1] := -1.0;
     b_alphaDot_l[2] := -2.0;   b_alphaDot_u[2] :=  0.0;
     b_alphaDot_l[3] := -1.0;   b_alphaDot_u[3] :=  1.0;
     b_alphaDot_l[4] :=  0.0;   b_alphaDot_u[4] :=  2.0;
     b_alphaDot_l[5] :=  1.0;   b_alphaDot_u[5] :=  3.0;
     b_alphaDot_l[6] :=  1.0;   b_alphaDot_u[6] :=  5.0;

     (* Regeln definieren *)
     b_rule_alpha[0]:=PM; b_rule_alphaDot[0]:=ZE; b_rule_a[0]:=PM;
     b_rule_alpha[1]:=PS; b_rule_alphaDot[1]:=PS; b_rule_a[1]:=PS;
     b_rule_alpha[2]:=PS; b_rule_alphaDot[2]:=NS; b_rule_a[2]:=ZE;
     b_rule_alpha[3]:=NM; b_rule_alphaDot[3]:=ZE; b_rule_a[3]:=NM;
     b_rule_alpha[4]:=NS; b_rule_alphaDot[4]:=NS; b_rule_a[4]:=NS;
     b_rule_alpha[5]:=NS; b_rule_alphaDot[5]:=PS; b_rule_a[5]:=ZE;
     b_rule_alpha[6]:=ZE; b_rule_alphaDot[6]:=ZE; b_rule_a[6]:=ZE;

     (* Bezeichnungen fuer Indizes *)
     b_bez[0]:='NB';
     b_bez[1]:='NM';
     b_bez[2]:='NS';
     b_bez[3]:='ZE';
     b_bez[4]:='PS';
     b_bez[5]:='PM';
     b_bez[6]:='PB';
end;

(*********************************************************************)
function balance(alpha,alphadot: real): real;
var
   b_alpha: array[0..6] of real;
   b_alphaDot: array[0..6] of real;
   b_Fa: array[0..6] of real;
   max,mid: real;
   i: integer;

begin
     (* Gewichte von alpha ausrechnen *)
     for i:=0 to 6 do begin
         if (b_alpha_u[i] > alpha) and (alpha > b_alpha_l[i]) then begin
            mid := (b_alpha_u[i]+b_alpha_l[i])/2;
            if alpha>mid then begin
               (* linke Haelfte *)
               b_alpha[i] := 1-(alpha-mid)/(b_alpha_u[i]-mid);
            end else begin
               (* rechte Haelfte *)
               b_alpha[i] := (alpha-b_alpha_l[i])/(mid-b_alpha_l[i]);
            end;
         end else begin
             b_alpha[i]:=0.0;
         end;
     end;

     (* Gewichte von alphaDot ausrechnen *)
     for i:=0 to 6 do begin
         if (b_alphaDot_u[i] > alphaDot)
            and (alphaDot > b_alphaDot_l[i]) then begin
            mid := (b_alphaDot_u[i]+b_alphaDot_l[i])/2;
            if alphaDot>mid then begin
               (* rechte Haelfte *)
               b_alphaDot[i] := 1.0-(alphaDot-mid)/(b_alphaDot_u[i]-mid);
            end else begin
               (* linke Haelfte *)
               b_alphaDot[i] :=
                   (alphaDot-b_alphaDot_l[i])/(mid-b_alphaDot_l[i]);
            end;
         end else begin
             b_alphaDot[i]:=0.0;
         end;
     end;

     (* Regeln anwenden *)
     for i:=0 to 6 do begin
         b_Fa[i] := 0.0;
     end;
     for i:=0 to 6 do begin
            b_Fa[b_rule_a[i]] := maximum(b_Fa[b_rule_a[i]],
                                  min(b_alpha[b_rule_alpha[i]],
                                      b_alphaDot[b_rule_alphaDot[i]]));
     end;

     (* groessten b_Fa suchen *)
     max:=0.0;
     for i:=0 to 6 do begin
         max:=max + (i-3)*b_Fa[i];
     end;

     (* alpha ausgeben *)
     gotoxy(58,3); write('alpha:');
     for i:=0 to 6 do begin
         gotoxy(58,4+i);
         write(b_bez[i],': ',b_alpha[i]:5:3);
     end;

     (* alphaDot ausgeben *)
     gotoxy(70,3); write('alphaDot:');
     for i:=0 to 6 do begin
         gotoxy(70,4+i);
         write(b_bez[i],': ',b_alphaDot[i]:5:3);
     end;

     (* b_Fa ausgeben *)
     gotoxy(65,13); write('b_Fa:');
     for i:=0 to 6 do begin
         gotoxy(65,14+i);
         write(b_bez[i],': ',b_Fa[i]:5:3);
     end;

     (* max ausgeben *)
     gotoxy(65,22);
     write('max=',max:6:3);

     balance := -max*a0 + a0_cheat;
end;

(*********************************************************************)
begin
     clrscr;
     for n:=0 to 180 do begin
         alpha:=n*pi/180.0;
         kx:=Xcenter+round(Xscale*cos(alpha));
         ky:=Ycenter-round(Yscale*sin(alpha));
         gotoxy(kx,ky);
         write('.');
     end;
     gotoxy(Xcenter,Ycenter);
     write('+');

     balance_init;

     n        := 1;
     alphaDot := alphaDot0;
     alpha    := alpha0;
     Fa       := m*balance(alpha,alphaDot);
     Fg       := m*g;

     while (alpha>=0) and (alpha<=pi) do begin
           gotoxy(1,1);
           write(n:4,': ');
           write('alpha = ',(alpha*180/pi):7:3,'ø ');
           write('alphaDot = ',alphaDot:6:3,' ');
           write('deltaAlpha = ',(deltaAlpha*180/pi):6:3,'ø ');
           write('Fa = ',Fa:6:3,' ');
           write('                               ');
           writeln;

           gotoxy(kx,ky); write('.');
           kx:=Xcenter+round(Xscale*cos(alpha));
           ky:=Ycenter-round(Yscale*sin(alpha));
           gotoxy(kx,ky); write('@');

           (* Tastaturbehandlung *)
           if keypressed then begin
              c:=readkey;
              if byte(c)=0 then begin
                 c:=readkey;
                 case byte(c) of
                   77: begin
                            alphaDot:=alphaDot - Bumper_alphaDot;
                            alpha:=alpha-Bumper_alpha;
                       end;
                   75: begin
                            alphaDot:=alphaDot + Bumper_alphaDot;
                            alpha:=alpha+Bumper_alpha;
                       end;
                 end;
              end else begin
                 if c='p' then begin
                    while not keypressed do;
                 end;
              end;
           end;

           Fa := m*balance(alpha,alphaDot);

           (* Pendel-Verhalten nach Ch. Ziegaus *)
           Fres       := sqrt(sqr(Fg) + sqr(Fa));
           gamma      := alpha - arccos(Fa/Fres);
           Fz         := sin(gamma) * Fres;
           deltaAlpha := Fz/(2.0*m*l)*sqr(deltaT) + alphaDot*deltaT;
           alphaDot   := Fz/(m*l)*deltaT + alphaDot;
           alpha      := alpha + deltaAlpha;

           n          := n+1;

           (* while not keypressed do;*)
     end;

     gotoxy(1,25);
     write('CR:');
     readln;
end.