with CursAKD; use CursAKD;
     with ConsAKD; use ConsAKD;
     with DOS_011_Plot; use DOS_011_Plot;
     with ErroAKD; use ErroAKD;
     with FloaAKD; use FloaAKD; -- for Demo
     with MathAK ; -- for Pi
     with MathAKD; use MathAKD;

     package body DOS_012_Fourier is
     ----------------------------------------------------------------------
     ----------------------------------------------------------------------

     procedure Correlation (
       Ans   : in out Vector_Type;
       Data1 : in     Vector_Type;
       Data2 : in     Vector_Type;
       N     : in     integer ) is
     ----------------------------------------------------------------------
       No2, I, II : integer;
       Dum        : float;
       FFT        : Vector_Type ( 1..( 2 * N ) );
     begin
       TwoFFT ( FFT, Ans, N, Data1, Data2 );
       No2 := N / 2;
       for I in 1..( No2 + 1 ) loop
	 II := 2 * I;
	 Dum := Ans ( II - 1 );
	 Ans ( II - 1 ) := ( FFT ( II - 1 ) * Ans ( II - 1 )
	   + FFT ( II ) * ANS ( II ) ) / float ( No2 );
	 Ans ( II ) := ( FFT ( II ) * Dum - FFT ( II - 1 ) * Ans ( II ) )
	   / float ( No2 );
       end loop;
       Ans ( 2 ) := Ans ( N + 1 );
       RealFT ( Ans, No2, -1 );
     exception
       when others =>
	 Notify ( "DOS_012_Fourier.Correlation" );
	 raise;
     end Correlation;

     procedure Four1 (
       Data  : in out Vector_Type;
       NN    : in     integer;
       ISign : in     integer := +1 ) is
     ----------------------------------------------------------------------
       II, JJ, N, MMax, M, J, IStep, I : integer;
       WTemp, Wr, Wpr, Wpi, Wi, Theta  : float;
       TempR, TempI                    : float;
     begin
       N := 2 * NN;
       J := 1;
       for II in 1..NN loop
	 I := 2 * II - 1;
	 if J > 1 then
	   TempR := Data ( J );        -- real component
	   TempI := Data ( J + 1 );    -- imaginary component
	   Data ( J ) := Data ( I );
	   Data ( J + 1 ) := Data ( I + 1 );
	   Data ( I ) := TempR;
	   Data ( I + 1 ) := TempI;
	 end if;
	 M := N / 2;
	 while ( M >= 2 ) and ( J > M ) loop
	   J := J - M;
	   M := M / 2;
	 end loop;
	 J := J + M;
       end loop;
       MMax := 2;
       while ( N > MMax ) loop
	 IStep := 2 * MMax;
	 Theta := 2.0 * MathAK.Pi / float ( ISign * MMax );
	 Wpr := -2.0 * Sqr ( Sin ( 0.5 * Theta ) );
	 Wpi := Sin ( Theta );
	 Wr := 1.0;
	 Wi := 0.0;
	 for II in 1..( MMax / 2 ) loop
	   M := 2 * II - 1;
	   for JJ in 0..( ( N-M ) / IStep ) loop
	     I := M + JJ * IStep;
	     J := I + MMax;
	     TempR := Sngl ( Wr ) * Data ( J )
	       - Sngl ( Wi ) * Data ( J + 1 );
	     Tempi := Sngl ( Wr ) * Data ( J + 1 )
	       + Sngl ( Wi ) * Data ( J );
	     Data ( J ) := Data ( I ) - TempR;
	     Data ( J + 1 ) := Data ( I + 1 ) - Tempi;
	     Data ( I ) := Data ( I ) + TempR;
	     Data ( I + 1 ) := Data ( I + 1 ) + Tempi;
	   end loop;
	   WTemp := Wr;
	   Wr := Wr * Wpr - Wi * Wpi + Wr;
	   Wi := Wi * Wpr + WTemp * Wpi + Wi;
	 end loop;
	 MMax := IStep;
       end loop;
     exception
       when others =>
	 Notify ( "DOS_012_Fourier.Four1" );
	 raise;
     end Four1;

     procedure RealFT (
       Data  : in out Vector_Type;
       N     : in     integer;
       ISign : in     integer ) is
     ----------------------------------------------------------------------
       Wr, Wi, Wpr, Wpi, Wtemp, Theta : float;
       I, I1, I2, I3, I4 : integer;
       C1, C2, H1r, H1i, H2r, H2i, Wrs, Wis : float;
     begin
       Theta := 2.0 * MathAK.Pi / ( 2.0 + float ( N ) );
       C1 := 0.5;
       if ISign = 1 then
	 C2 := -0.5;
	 Four1 ( Data, N, 1 );
       else
	 C2 := 0.5;
	 Theta := -Theta;
       end if;
       Wpr := -2.0 * Sqr ( Sin ( 0.5 * Theta ) );
       Wpi := Sin ( Theta );
       Wr := 1.0 + Wpr;
       Wi := Wpi;
       for I in 2..( N / 2 ) loop
	 I1 := I + I - 1;
	 I2 := I1 + 1;
	 I3 := N + N + 3 - I2;
	 I4 := I3 + 1;
	 Wrs := Sngl ( Wr );
	 Wis := Sngl ( Wi );
	 H1r :=  C1 * ( Data ( I1 ) + Data ( I3 ) );
	 H1i :=  C1 * ( Data ( I2 ) - Data ( I4 ) );
	 H2r := -C2 * ( Data ( I2 ) + Data ( I4 ) );
	 H2i :=  C2 * ( Data ( I1 ) - Data ( I3 ) );
	 Data ( I1 ) := H1r - Wrs * H2r - Wis * H2i;
	 Data ( I2 ) := H1i + Wrs * H2i + Wis * H2r;
	 Wtemp := Wr;
	 Wr := Wr * Wpr - Wi * Wpi + Wr;
	 Wi := Wi * Wpr + Wtemp * Wpi + Wi;
       end loop;
       if ISign = 1 then
	 H1r := Data ( 1 );
	 Data ( 1 ) := H1r + Data ( 2 );
	 Data ( 2 ) := H1r - Data ( 2 );
       else
	 H1r := Data ( 1 );
	 Data ( 1 ) := C1 * ( H1r + Data ( 2 ) );
	 Data ( 2 ) := C1 * ( H1r - Data ( 2 ) );
	 Four1 ( Data, N, -1 );
       end if;
     exception
       when others =>
	 Notify ( "DOS_012_Fourier.RealFT" );
	 raise;
     end RealFT;

     function Sngl ( F : float ) return float is
     ----------------------------------------------------------------------
     begin
       return F;
     end Sngl;

     function Sqr ( F : float ) return float is
     ----------------------------------------------------------------------
     begin
       return F ** 2;
     end Sqr;

     procedure TwoFFT (
       FFT1  : in out Vector_Type;
       FFT2  : in out Vector_Type;
       N     : in     integer;
       Data1 : in     Vector_Type;
       Data2 : in     Vector_Type ) is
     ----------------------------------------------------------------------
       NN3, NN2, NN, JJ, J : integer;
       Rep, Rem1, Aip, Aim  : float;
     begin
       NN := N + N;
       NN2 := NN + 2;
       NN3 := NN + 3;
       for J in 1..N loop
	 JJ := J + J;
	 FFT1 ( JJ - 1 ) := Data1 ( J );
	 FFT1 ( JJ ) := Data2 ( J );
       end loop;
       Four1 ( FFT1, N, 1 );
       FFT2 ( 1 ) := FFT1 ( 2 );
       FFT1 ( 2 ) := 0.0;
       FFT2 ( 2 ) := 0.0;
       for JJ in 1..( N / 2 ) loop
	 J := 2 * JJ + 1;
	 Rep := 0.5 * ( FFT1 ( J ) + FFT1 ( NN2 - J ) );
	 Rem1 := 0.5 * ( FFT1 ( J ) - FFT1 ( NN2 - J ) );
	 Aip := 0.5 * ( FFT1 ( J + 1 ) + FFT1 ( NN3 - J ) );
	 Aim := 0.5 * ( FFT1 ( J + 1 ) - FFT1 ( NN3 - J ) );
	 FFT1 ( J ) := Rep;
	 FFT1 ( J + 1 ) := Aim;
	 FFT1 ( NN2 - J ) := Rep;
	 FFT1 ( NN3 - J ) := -Aim;
	 FFT2 ( J ) := Aip;
	 FFT2 ( J + 1 ) := -Rem1;
	 FFT2 ( NN2 - J ) := Aip;
	 FFT2 ( NN3 - J ) := Rem1;
       end loop;
     exception
       when others =>
	 Notify ( "DOS_012_Fourier.TwoFFT" );
	 raise;
     end TwoFFT;
     ----------------------------------------------------------------------
     ----------------------------------------------------------------------

     function Complex ( R, I : Vector_Type ) return Vector_Type is
     ----------------------------------------------------------------------
       Temp : Vector_Type ( 1..( 2 * R'last ) );
     begin
       for Index in R'range loop
	 Temp ( 2 * ( Index - 1 ) + 1 ) := R ( Index );
	 Temp ( 2 * ( Index - 1 ) + 2 ) := I ( Index );
       end loop;
       return Temp;
     end Complex;

     procedure Demo is
     ----------------------------------------------------------------------
       P : constant positive := 9;
       N : constant positive := 2 ** P;
       Ans  : Vector_Type ( 1..( 2 * N ) );
       Data : Vector_Type ( 1.. ( 2 * N ) );
       Data_Original : Vector_Type ( Data'range );
       Freq : float := 1.0;  -- Hz
       Ticks : Vector_Type ( 1..N );
       Data_Real, Data_Imag, Data_Freq : Vector_Type ( 1..N );
     begin
       Freq := Ask ( "Frequency:  ", Freq );
       for Tick in Ticks'range loop
	 Ticks ( Tick ) := float ( Tick ); -- /float ( N )
       end loop;
       for Time in Data'range loop
	 if Time rem 2 = 1 then
	   Data ( Time ) := Sin ( 2.0 * MathAK.Pi * Freq * float ( Time )
	     / float ( 2 * N ) );
	 else
	   Data ( Time ) := 0.0;
	 end if;
--       Put ( float ( Time ) );
--       Put ( float ( Time rem 2 ) );
--       Put_Line ( Data ( Time ), Aft => 5 );
--       Pause;
       end loop;
       Data_Original := Data;
       Clear_Screen;
       Plot_XY ( Ticks, Imaginary ( Data ), Symbol => '.' );
       Plot_XY ( Ticks, Real ( Data ) );
       Correlation ( Ans, Data, Data, N );
       Save ( Ans, "AutoCorr.Dat" );
       Move ( 24, 0 );
       Pause;
       Clear_Screen;
       Plot_XY ( Ticks, Ans );
       Four1 ( Data, N, +1 );
       Data_Real := Real ( Data );
       for Index in 1..( N / 2 ) loop
	 Data_Freq ( Index ) := Data_Real ( Index + N / 2 );
	 Data_Freq ( Index + N / 2 ) := Data_Real ( Index );
       end loop;
       Move ( 24, 0 );
       Pause;
       Clear_Screen;
--     Plot_XY ( Ticks, Imaginary ( Data ), Symbol => '.' );
--     Plot_XY ( Ticks, Real ( Data ) );
       Plot_XY ( Ticks, Data_Freq );
       Four1 ( Data, N, -1 );
       Data := Data / float ( N );
       Move ( 24, 0 );
       Pause;
       Clear_Screen;
       Plot_XY ( Ticks, Imaginary ( Data ), Symbol => '.' );
       Plot_XY ( Ticks, Real ( Data ) );
       Correlation ( Ans, Data_Original, Data_Original, N );
       Move ( 24, 0 );
       Pause;
       Clear_Screen;
       Plot_XY ( Ticks, Imaginary ( Ans ), Symbol => '.' );
       Plot_XY ( Ticks, Real ( Ans ) );
       Data := ( others => 0.0 );
       Data ( 3 ) := 1.0;
       Data ( 2 * N - 1 ) := 1.0;
       Move ( 24, 0 );
       Pause;
       Clear_Screen;
       Plot_XY ( Ticks, Imaginary ( Data ), Symbol => '.' );
       Plot_XY ( Ticks, Real ( Data ) );
       Four1 ( Data, N, -1 );
       Move ( 24, 0 );
       Pause;
       Clear_Screen;
       Plot_XY ( Ticks, Imaginary ( Data ), Symbol => '.' );
       Plot_XY ( Ticks, Real ( Data ) );
       Four1 ( Data, N, +1 );
       Move ( 24, 0 );
       Pause;
       Clear_Screen;
       Plot_XY ( Ticks, Imaginary ( Data ), Symbol => '.' );
       Plot_XY ( Ticks, Real ( Data ) );
       Move ( 24, 0 );
       Pause;
     end Demo;

     function Imaginary ( V : Vector_Type ) return Vector_Type is
     ----------------------------------------------------------------------
       I : Vector_Type ( 1..( V'last / 2 ) );
     begin
       for Index in I'range loop
	 I ( Index ) := V ( 2 * ( Index - 1 ) + 2 );
       end loop;
       return I;
     end Imaginary;

     function Real ( V : Vector_Type ) return Vector_Type is
     ----------------------------------------------------------------------
       R : Vector_Type ( 1..( V'last / 2 ) );
     begin
       for Index in R'range loop
	 R ( Index ) := V ( 2 * ( Index - 1 ) + 1 );
       end loop;
       return R;
     end Real;

     ----------------------------------------------------------------------
     ----------------------------------------------------------------------
     end DOS_012_Fourier;