{$IFDEF Energie}
{$IFDEF Gradient}
 Ich kann nur Energie _oder_ Gradient berechnen!
 Bitte Compilerschalter richtig setzen!
{$ENDIF}
{$ENDIF}

{$IFNDEF Energie}
{$IFNDEF Gradient}
 Ich muss mindestens Energie oder Gradient berechnen!
 Bitte Compilerschalter richtig setzen!
{$ENDIF}
{$ENDIF}

// berechnet die Energie oder den Gradienten
var
 i,j,k:      Longint;
 tmp,tmp2:   extended;
{$IFDEF Gradient}
 px,py,tmp3: extended;
{$ENDIF}

begin

 // Initialisierung
 {$IFDEF Energie}
 Kreuzungen:=0;
 Kreuzung:=-1;
 result:=0;
 {$ENDIF}
 {$IFDEF Gradient}
 if length(grad)<>length(p) then begin
   writeln('*** Warnung ***');
   writeln('Länge das Gradienten wurde angepasst!');
   setlength(grad,length(p));
 end;
 for i:=0 to length(grad)-1 do
   grad[i]:=0;
 {$ENDIF}

 {$IFDEF detaillierteZeitanalyse}
 if mainThread then ffTimer.start;
 {$ENDIF}

 for I:=0 to length(FFInteraktionen)-1 do
   with FFInteraktionen[I] do
     // auf Kreuzung prüfen:
     // ((B1-A1)x(A2-A1))*((B2-A1)x(A2-A1))<=0 und
     // ((A1-B1)x(B2-B1))*((A2-B1)x(B2-B1))<=0 <=> Kreuzung
     if (KreuzproduktZ(P,_Fs[1].Eltern[0],_Fs[0].Eltern[0],_Fs[0].Eltern[1],_Fs[0].Eltern[0]) *
         KreuzproduktZ(P,_Fs[1].Eltern[1],_Fs[0].Eltern[0],_Fs[0].Eltern[1],_Fs[0].Eltern[0]) <= 0 ) and
        (KreuzproduktZ(P,_Fs[0].Eltern[0],_Fs[1].Eltern[0],_Fs[1].Eltern[1],_Fs[1].Eltern[0]) *
         KreuzproduktZ(P,_Fs[0].Eltern[1],_Fs[1].Eltern[0],_Fs[1].Eltern[1],_Fs[1].Eltern[0]) <= 0 )
      then begin
        {$IFDEF Energie}
        inc(Kreuzungen);
        if (Kreuzung=-1) or
           (random*Kreuzungen<1) then Kreuzung:=i;     // eine Kreuzung zufällig, gleichverteilt auswählen
        {$ENDIF}

        // jetzt soll belohnt (!) werden, wenn ein Elternteil der einen Familie nah an der "Kante" der anderen Familie liegt
        //  -> dadurch kann eine Energie-Optimierung auch Kreuzungen entfernen
        for j:=0 to 1 do             // Familie
          for k:=0 to 1 do begin     // Elter
            tmp:=(1e-9  + (QAbstand(P,_FS[j].Eltern[k],_FS[1-j].Eltern[0])
                        - ( sqr(Skalarprodukt(P,_FS[j].Eltern[k],_FS[1-j].Eltern[0],_FS[1-j].Eltern[1],_FS[1-j].Eltern[0]))
                          / (1e-9+QAbstand(P,_FS[1-j].Eltern[1],_FS[1-j].Eltern[0])))));
            {$IFDEF Energie}
            result:=result - Entkreuzungsbelohnungsskalierung * 1/tmp;
            {$ENDIF}
            {$IFDEF Gradient}
            px:= P[_FS[1-j].Eltern[1].p2]-P[_FS[1-j].Eltern[0].p2];
            py:=-P[_FS[1-j].Eltern[1].p1]+P[_FS[1-j].Eltern[0].p1];
            tmp2:=1/sqrt(sqr(px)+sqr(py)+epsilon);
            if (px*(P[_FS[j].Eltern[k].p1]-P[_FS[1-j].Eltern[0].p1])+py*(P[_FS[j].Eltern[k].p2]-P[_FS[1-j].Eltern[0].p2]) <= 0) then
              tmp2:=-tmp2;
            px:=px*tmp2;
            py:=py*tmp2;
            tmp2:=Entkreuzungsbelohnungsskalierung*2/sqr(tmp)*sqrt(tmp);

            tmp:=Skalarprodukt(P,_FS[j].Eltern[k],_FS[1-j].Eltern[0],_FS[1-j].Eltern[1],_FS[1-j].Eltern[0]) /    // (P-F1)*(F2-F1) / (F2-F1)^2
                     (1e-9+QAbstand(P,_FS[1-j].Eltern[1],_FS[1-j].Eltern[0]));

            grad[_FS[j].Eltern[k].p1]:=               // Ableitungen nach Position der Person
              grad[_FS[j].Eltern[k].p1] + tmp2*px;
            grad[_FS[j].Eltern[k].p2]:=
              grad[_FS[j].Eltern[k].p2] + tmp2*py;

            grad[_FS[1-j].Eltern[0].p1]:=             // Ableitungen nach Position der Eltern
              grad[_FS[1-j].Eltern[0].p1] - tmp2*(1-tmp)*px;
            grad[_FS[1-j].Eltern[0].p2]:=
              grad[_FS[1-j].Eltern[0].p2] - tmp2*(1-tmp)*py;

            grad[_FS[1-j].Eltern[1].p1]:=
              grad[_FS[1-j].Eltern[1].p1] - tmp2*tmp*px;
            grad[_FS[1-j].Eltern[1].p2]:=
              grad[_FS[1-j].Eltern[1].p2] - tmp2*tmp*py;
            {$ENDIF}
          end;
      end;
 {$IFDEF detaillierteZeitanalyse}
 if mainThread then begin
   ffTimer.stop;
   mmTimer.start;
 end;
 {$ENDIF}

 for I:=0 to length(MMInteraktionen)-1 do
   with MMInteraktionen[I] do begin  // 1/x^2 - Abstoßung
     {$IFDEF Energie}
     result:=result +
       Laenge/(1e-9 + QAbstand(P,_Ps[0],_Ps[1]));
     {$ENDIF}
     {$IFDEF Gradient}
     tmp:=1/(1e-9 + QAbstand(P,_Ps[0],_Ps[1]));
     tmp:=-Laenge*2*sqr(tmp);
     grad[_Ps[0].p1]:=
       grad[_Ps[0].p1] + tmp * (P[_Ps[0].p1]-P[_Ps[1].p1]);
     grad[_Ps[0].p2]:=
       grad[_Ps[0].p2] + tmp * (P[_Ps[0].p2]-P[_Ps[1].p2]);
     grad[_Ps[1].p1]:=
       grad[_Ps[1].p1] + tmp * (P[_Ps[1].p1]-P[_Ps[0].p1]);
     grad[_Ps[1].p2]:=
       grad[_Ps[1].p2] + tmp * (P[_Ps[1].p2]-P[_Ps[0].p2]);
     {$ENDIF}
   end;

 {$IFDEF detaillierteZeitanalyse}
 if mainThread then begin
   mmTimer.stop;
   mfTimer.start;
 end;
 {$ENDIF}

 for I:=0 to length(MFInteraktionen)-1 do
   with MFInteraktionen[I] do begin
     tmp:= Skalarprodukt(P,_P,_F.Eltern[0],_F.Eltern[1],_F.Eltern[0]) /    // (P-F1)*(F2-F1) / (F2-F1)^2
             (epsilon+QAbstand(P,_F.Eltern[1],_F.Eltern[0]));
     if (tmp<=0) or (tmp>=1) then begin // Person liegt "vor"/"hinter" Familie -> Abstand zu Elter relevant
       tmp2:=epsilon + QAbstand(P,_P,_F.Eltern[byte(tmp>0.5)]);
       {$IFDEF Gradient}
       tmp3:=-2*Laenge/sqr(tmp2);
       grad[_P.p1]:=
         grad[_P.p1] + tmp3*(P[_P.p1]-P[_F.Eltern[byte(tmp>0.5)].p1]);
       grad[_P.p2]:=
         grad[_P.p2] + tmp3*(P[_P.p2]-P[_F.Eltern[byte(tmp>0.5)].p2]);
       grad[_F.Eltern[byte(tmp>0.5)].p1]:=
         grad[_F.Eltern[byte(tmp>0.5)].p1] + tmp3*(P[_F.Eltern[byte(tmp>0.5)].p1]-P[_P.p1]);
       grad[_F.Eltern[byte(tmp>0.5)].p2]:=
         grad[_F.Eltern[byte(tmp>0.5)].p2] + tmp3*(P[_F.Eltern[byte(tmp>0.5)].p2]-P[_P.p2]);
       {$ENDIF}
     end
     else begin                        // Person liegt "neben" Familie -> senkrechter Abstand relevant
       tmp2:=1e-9 + QAbstand(P,_P,_F.Eltern[0]) -                        // Pythagoras
                    sqr(tmp)*QAbstand(P,_F.Eltern[1],_F.Eltern[0]);
       {$IFDEF Gradient}
       px:=-P[_F.Eltern[1].p2]+P[_F.Eltern[0].p2];
       py:= P[_F.Eltern[1].p1]-P[_F.Eltern[0].p1];
       tmp3:=1/sqrt(sqr(px)+sqr(py)+epsilon);
       if (px*(P[_P.p1]-P[_F.Eltern[0].p1])+py*(P[_P.p2]-P[_F.Eltern[0].p2]) <= 0) then
         tmp3:=-tmp3;
       px:=px*tmp3;
       py:=py*tmp3;

       tmp2:=-2*Laenge/sqr(tmp2)*sqrt(tmp2);

       grad[_P.p1]:=               // Ableitungen nach Position der Person
         grad[_P.p1] + tmp2*px;
       grad[_P.p2]:=
         grad[_P.p2] + tmp2*py;

       grad[_F.Eltern[0].p1]:=     // Ableitungen nach Position der Eltern
         grad[_F.Eltern[0].p1] - tmp2*(1-tmp)*px;
       grad[_F.Eltern[0].p2]:=
         grad[_F.Eltern[0].p2] - tmp2*(1-tmp)*py;

       grad[_F.Eltern[1].p1]:=
         grad[_F.Eltern[1].p1] - tmp2*tmp*px;
       grad[_F.Eltern[1].p2]:=
         grad[_F.Eltern[1].p2] - tmp2*tmp*py;
       {$ENDIF}
     end;

     {$IFDEF Energie}
     result:=result + Laenge/tmp2;
     {$ENDIF}
   end;

 {$IFDEF detaillierteZeitanalyse}
 if mainThread then begin
   mfTimer.stop;
   famTimer.start;
 end;
 {$ENDIF}

 {$IFDEF Energie}
 for i:=0 to length(Familien)-1 do
   result:=result +
     Laengengewicht *
       sqr(sqr(QAbstand(P,Familien[i].Eltern[0],Familien[i].Eltern[1])));
 {$ENDIF}
 {$IFDEF Gradient}
 for i:=0 to length(Familien)-1 do begin
   tmp:=QAbstand(P,Familien[i].Eltern[0],Familien[i].Eltern[1]);
   tmp:=8*Laengengewicht*sqr(tmp)*tmp; // 8 (a-b)^6
   grad[Familien[i].Eltern[0].p1]:=
     grad[Familien[i].Eltern[0].p1] + tmp*(P[Familien[i].Eltern[0].p1]-P[Familien[i].Eltern[1].p1]);
   grad[Familien[i].Eltern[0].p2]:=
     grad[Familien[i].Eltern[0].p2] + tmp*(P[Familien[i].Eltern[0].p2]-P[Familien[i].Eltern[1].p2]);
   grad[Familien[i].Eltern[1].p1]:=
     grad[Familien[i].Eltern[1].p1] + tmp*(P[Familien[i].Eltern[1].p1]-P[Familien[i].Eltern[0].p1]);
   grad[Familien[i].Eltern[1].p2]:=
     grad[Familien[i].Eltern[1].p2] + tmp*(P[Familien[i].Eltern[1].p2]-P[Familien[i].Eltern[0].p2]);
 end;
 {$ENDIF}

 {$IFDEF detaillierteZeitanalyse}
 if mainThread then famTimer.stop;
 {$ENDIF}

end;