unit u_calc;

// Purpose: Several Calculations

{$mode objfpc}{$H+}

interface

uses Classes, SysUtils;

var
  EllipseMinorAxis_mm, EllipseMajorAxis_mm,
  PrecessionAngle_Degrees: real;
  Points: longword;
  CompassCalibrationScale: word;

// for the extra calculation of the "Quadrature Method"
  QPoints: integer;
  QPrevPositionCounter: longword;
  QSumCosX, QSumSinX, QSumCosY, QSumSinY,
  QAmplitudeX, QAmplitudeY, QAngleX, QAngleY: real;

// for Amplitude Control
  SetPoint_Amplitude_Ticks: word;

// for data Logging
 TPassCenter_Cap_HFSW1, TPassCenter_Cap_HFSW2,
 TPassCenter_Mag_HFSW1, TPassCenter_Mag_HFSW2,
 APeakRim1_Mag_HFSW1, APeakRim1_Mag_HFSW2,
 TPassRim1_Mag_HFSW1, TPassRim1_Mag_HFSW2: word;

// We list all functions and procedures in this unit
// in the order they appear below.
procedure ProcessPositionData;
procedure CalulateQuadratureData (PosX, PosY: real; PositionCounter: integer; HalfSwing: boolean);
procedure RecalculateFrequencies;
procedure RecalculateAmplitudeSettings;

implementation

uses u_main, u_messages, u_logging, u_compass, u_numstrings, math, graphics;

var
  PosNS, PosEW, CenterPosNS, CenterPosEW, PrevPosNS, PrevPosEW,
  SumPosNS, SumPosEW,
  avZeroCorrectionNS, avZeroCorrectionEW,
  DistanceSquared, PrevDistanceSquared,
  avFarestPosNS, avFarestPosEW,
  avMinorAxisNS, avMinorAxisEW,
  EllipseMinorAxis_pms, EllipseMajorAxis_pms,
  EllipseRatio_Percent,
  EllipseCalibrationFactor_Auto,
  PrecessionAngle_Radians: real;
  PeriodTime_Mag_ticks, CompassScale: word;
  FoundFarest, SeenCenter1, SeenCenter2: boolean;

procedure ProcessPositionData; // executed at each message arrival
const
  avFactor= 0.1; // for averaging the farest position
  TargetGainCalibration = 700; // for bob in rest in the center
var
  RotX, RotY: real;
begin
  // we first find out if we have a CenterPass in the First or in the Second HalfSwing
  if Seencenter1 then SeenCenter1:= false;  // only once
  if SeenCenter and HalfSwing then SeenCenter1:= true;
  if SeenCenter2 then SeenCenter2:= false;  // only once
  if SeenCenter and (not HalfSwing) then SeenCenter2:= true;
  SeenCenter:= false;
  //if SeenCenter1 then MemoAdd ('SeenCenter1');
  //if SeenCenter2 then MemoAdd ('SeenCenter2');

  FrmMain.TbxTargetGainCalibration.Text:= format ('%4d', [TargetGainCalibration]);
  if CalibratePMS then
  begin
    // For adjusting the gains, while Bob is still, we use the
    // enlargement scale for fine adjustment.
    if Enlarge then CompassCalibrationScale:= 10 else CompassCalibrationScale:= 2;
    if not Persistence then Compass.Init (FrmMain, Compass_Left, Compass_Top, Compass_Size);
    Compass.PlotFilledBox ( 0, (Adc_North - TargetGainCalibration) *  CompassCalibrationScale,    3, clLime);   // color northern light
    Compass.PlotFilledBox ( 0, (Adc_South - TargetGainCalibration) * -CompassCalibrationScale,    3, clAqua);   // color ice
    Compass.PlotFilledBox ((Adc_East - TargetGainCalibration) *       CompassCalibrationScale, 0, 3, clYellow); // color sunrise
    Compass.PlotFilledBox ((Adc_West - TargetGainCalibration) *      -CompassCalibrationScale, 0, 3, clRed);    // color sunset
  end
  else
  begin
    // Calculate normalized positions from Position sensors
    // Normalized means they are between -1 and +1
    // The position sensors increase their output when the wire approaches the sensorplate.
    if (Adc_North + Adc_South) > 0 then  // prevent div by zero crash on bad data
      PosNS:= (real (Adc_North - Adc_South) / real (Adc_North + Adc_South))
    else PosNS:= 0.0;
    if (Adc_East + Adc_West) > 0 then     // prevent div by zero crash on bad data
      PosEW:= (real (Adc_East - Adc_West) / real (Adc_East + Adc_West))
    else PosEW:= 0.0;

    // we sum during a full swing for calculation of autozero cerrection
    SumPosNS += PosNS;
    SumPosEW += PosEW;
    inc (Points);
    if SeenCenter1 then // start a new averaging cycle
    begin
      // MemoAdd ('FullSwing');
      // we have a full swing now. The sum of all datapoints forms the required correction
      // apply leaking Bucket averaging;
      PMSZeroAverageFactor:= fRval (FrmMain.TbxPMSZeroAverageFactor.text);
      avZeroCorrectionNS *= (1.0 - PMSZeroAverageFactor);
      avZeroCorrectionNS += SumPosNS / Points * PMSZeroAverageFactor;
      avZeroCorrectionEW *= (1.0 - PMSZeroAverageFactor);
      avZeroCorrectionEW += SumPosEW / Points * PMSZeroAverageFactor;
      FrmMain.TbxAutoZeroNS.text:= format ('%8.5f', [avZeroCorrectionNS]);
      FrmMain.TbxAutoZeroEW.text:= format ('%8.5f', [avZeroCorrectionEW]);
      SumPosNS:= 0;
      SumPosEW:= 0;
      Points:= 0;
    end;
    // Note that if there is a zero offset, the calculation of the minor axis length will be in error
    if AutoZeroPositionData then
    begin
      // apply correction
      PosNS -= avZeroCorrectionNS;
      PosEW -= avZeroCorrectionEW;
    end;

    // show normalized positions
    FrmMain.TbxPosNS.text:= format ('%5.3f', [PosNS]);
    Frmmain.TbxPosEW.text:= format ('%5.3f', [PosEW]);

    CalulateQuadratureData (PosNS, PosEW, PositionCounter, HalfSwing);

    // Plot on Compass Display
    CompassScale:= fIval (FrmMain.TbxCompassScale.text);
    if Enlarge then CompassScale *= 4;
    if not Persistence then
    begin // remove previous blip
      Compass.PlotPoint (round (PrevPosEW * CompassScale), round (PrevPosNS * CompassScale), 1, clBlack);
      Compass.ReDraw; // redraw grid
    end;
    // save previous positions for removal in plot and for precession angle calculation.
    PrevPosNS:= PosNS;
    PrevPosEW:= PosEW;

    if not HalfSwing then // show the first halfswing in color white
      Compass.PlotPoint (round (PosEW * CompassScale), round (PosNS * CompassScale), 1, clWhite) // new blip
    else
    begin
      // show the second halfswing in color light blue
      Compass.PlotPoint (round (PosEW * CompassScale), round (PosNS * CompassScale), 1, clAqua);

      // We do further processing only in the active Halfswing
      if SeenCenter1 then // prepare for searching the farest datapoint
      begin
        // MemoAdd ('SeenCenter');
        // prepare search for farest datapoint which indicates the Precession Angle
        PrevDistanceSquared:= 0; // to start a new search
        FoundFarest:= false;
        // Here we also calculate the normalized position for the CenterPass Position
        // The Center Pass Position Data are frozen at the moment of detecting a CenterPass
        if (Center_North + Center_South) > 0 then // prevent div by zero crash on bad data
          CenterPosNS:= (real (Center_North - Center_South) / real (Center_North + Center_South))
        else CenterPosNS:= 0.0;
        if (Center_East + Center_West) > 0 then   // prevent div by zero crash on bad data
          CenterPosEW:= (real (Center_East - Center_West) / real (Center_East + Center_West))
        else CenterPosEW:= 0.0;
        if AutoZeroPositionData then
        begin
          // apply correction
          CenterPosNS -= avZeroCorrectionNS;
          CenterPosEW -= avZeroCorrectionEW;
        end;
        // Plot a green square at the point of the CenterPass
        if FrmMain.CbxShowCenterPassBlip.checked then
          Compass.PlotFilledBox (round (CenterPosEW * CompassScale), round (CenterPosNS * CompassScale), 3, clLime);
      end;

      // To find the Precession Angle we search for the farest distance from the center
      if not FoundFarest then
      begin
        DistanceSquared:= sqr (PosEW) + sqr (PosNS); // we dont need the squareroot now
        if DistanceSquared >= PrevDistanceSquared then // track increasing distance
          PrevDistanceSquared:= DistanceSquared
        else // we are on the way back, so the previous position was the farest away
        begin
          FoundFarest:= true;
          //MemoAdd ('Found Farest');
          // Enter the farest coordinates into leaking bucket averagers
          avFarestPosNS:= avFarestPosNS * (1-avFactor) + PrevPosNS * avFactor;
          avFarestPosEW:= avFarestPosEW * (1-avFactor) + PrevPosEW * avFactor;
          // plot the farest position found
          if FrmMain.CbxShowFarestBlip.checked then
            Compass.PlotFilledBox (round (PrevPosEW * CompassScale),
                                   round (PrevPosNS * CompassScale), 3, clRed);
          // plot the farest position averaged
          if FrmMain.CbxShowAveragedFarestBlip.checked then
            Compass.PlotFilledBox (round (avFarestPosEW * CompassScale),
                                   round (avFarestPosNS * CompassScale), 3, clAqua);
          FrmMain.TbxFarestNS.text:= format ('%5.3f', [avFarestPosNS]);
          FrmMain.TbxFarestEW.text:= format ('%5.3f', [avFarestPosEW]);
          // calculate the angle
          PrecessionAngle_Radians:= arctan2 (avFarestPosNS, avFarestPosEW);
          PrecessionAngle_Degrees:= PrecessionAngle_Radians * 180 / pi();
          FrmMain.TbxPrecessionAngle.text:= format ('%5.1f', [PrecessionAngle_Degrees]);
          // plot the red circle
          Compass.PlotAngleMarker (PrecessionAngle_Degrees, clRed);
        end;
      end;

      // We calculate the Ellipse Minor Axis from the data frozen at the Center Pass
      if Seencenter1 then
      begin
        // put the Centerposition Data into the leaking bucket averagers
        avMinorAxisNS:= avMinorAxisNS * (1-avFactor) + CenterPosNS * avFactor;
        avMinorAxisEW:= avMinorAxisEW * (1-avFactor) + CenterPosEW * avFactor;
        // The minor axis is the distance between [0,0] and this sample
        EllipseMinorAxis_pms:= sqrt (sqr (avMinorAxisNS) + sqr (avMinorAxisEW)); // PMS units

        // To find the polarity of the minor axis, we rotate the sample (vector) over -PrecessionAngle.
        // Otherwise said: the ellipse is now laying horizontally (if all points were rotated).
        Rotate (avMinorAxisEW, avMinorAxisNS, -PrecessionAngle_Radians, RotX, RotY);
        // Plot the rotated CenterPass Position
        Compass.PlotPoint (round (RotX * CompassScale), round (RotY * CompassScale), 1, clRed);
        if RotY > 0 then EllipseMinorAxis_pms *= -1; // Positive rotation is CCW
        EllipseRatio_Percent:= 100.0 * abs (EllipseMinorAxis_pms / EllipseMajorAxis_pms);
        FrmMain.TbxEllipseRatio_Percent.text:= format ('%5.2f', [EllipseRatio_Percent]);

        // Calibrate the ellipse axis:
        EllipseMajorAxis_pms:= sqrt (PrevDistanceSquared); // the one before we found the farest
        if (AmplitudeControlMode = AmplitudeControlByRim_Mag) or
           (AmplitudeControlMode = AmplitudeControlByCenter_Cap) then
        begin
          // To have calibrated values for the Ellipse dimensions we take the major axis in PMS-units:
          // But we know that this is equal to the SetPoint, if Amplitude Control is working properly.
          if EllipseMajorAxis_pms > 0.000001 then // prevent div by zero crash if data fails
            EllipseCalibrationFactor_Auto:= SetPoint_Amplitude_mm / EllipseMajorAxis_pms;
          EllipseMinorAxis_mm:= EllipseMinorAxis_pms * EllipseCalibrationFactor_Auto;
          EllipseMajorAxis_mm:= SetPoint_Amplitude_mm;
        end;
        if AmplitudeControlMode = AmplitudeControlByNone then
        begin // we have to provide an explicite calibration factor
          EllipseMajorAxis_mm:= EllipseMajorAxis_pms * CalibrateEllipseAxis_mm;
          EllipseMinorAxis_mm:= EllipseMinorAxis_pms * CalibrateEllipseAxis_mm;
        end;
        FrmMain.TbxEllipseMajorAxis.text:= format ('%5.1f', [EllipseMajorAxis_mm]);
        FrmMain.TbxEllipseMinorAxis.text:= format ('%5.2f', [EllipseMinorAxis_mm]);
      end;
     end;
  end;
  // prepare data for logging
  // we log at each second HalfSwing, but we also need data from the first HalfSwing
  if SeenCenter1 then
  begin
    TPassCenter_Cap_HFSW1:= TPassCenter_Cap;
    TPassCenter_Mag_HFSW1:= TPassCenter_Mag;
    APeakRim1_Mag_HFSW1:= APeakRim1_Mag;
    TPassRim1_Mag_HFSW1:= TPassRim1_Mag;
  end;
  if SeenCenter2 then
  begin
    TPassCenter_Cap_HFSW2:= TPassCenter_Cap;
    TPassCenter_Mag_HFSW2:= TPassCenter_Mag;
    APeakRim1_Mag_HFSW2:= APeakRim1_Mag;
    TPassRim1_Mag_HFSW2:= TPassRim1_Mag;
    logData;
  end;

end;

// We do an extra calculation of the ellipse parameters, based on the dataset of
// one complete swing.
// This is the so called "Single Frequency Fourier Transformation" method or "Quadrature" method.
// It produces correct data, but the further processing is not yet worked out.
// When I have found out how to extract the ellipse data (major, minor, precession angle) from
// these data I will publish new code.

// This procedure is called for each datapoint. (message arrival)
procedure CalulateQuadratureData (PosX, PosY: real; PositionCounter: integer; Halfswing: boolean);
var
  Angle, SinAngle, CosAngle: real;
  PositionCounts: longword;
begin
  // because PositionCounter is nulled at each CenterPass we
  // add its last value to the other half
  if HalfSwing then
  begin
    PositionCounts:= PositionCounter; // during first HalfSwing
    QPrevPositionCounter:= PositionCounter; // track it until the second HalfSwing
  end
  else
  begin // during second HalfSwing
    PositionCounts:= PositionCounter + QPrevPositionCounter;
  end;
  // MemoAdd (format ('Quad %6d', [PositionCounts]));

  // We now have a PositionCounts (20 kHz samples) which goes from 0 to a full swing.
  // Full Swing has PeriodTime_ticks, so we can calculeat where we are in the swing period.
  Angle:= PositionCounts / PeriodTime_ticks * 2 * pi;
  SinAngle:= sin (Angle);
  CosAngle:= cos (Angle);
  QSumCosX += PosX * CosAngle;
  QSumSinX += PosX * SinAngle;
  QSumCosY += PosY * CosAngle;
  QSumSinY += PosY * SinAngle;
  QPoints += 1;  // we need to keep track of how many datapoints we collect

  if SeenCenter1 then // start of a new period
  begin
    //MemoAdd ('Finish and Start Over');
    QAmplitudeX:= sqrt (sqr (QSumCosX) + sqr (QSumSinX)) / QPoints;
    QAmplitudeY:= sqrt (sqr (QSumCosY) + sqr (QSumSinY)) / QPoints;
    QAngleX:= arctan2 (QSumSinX, QSumCosX);   // arctan2 works with Y,X over 2pi
    QAngleY:= arctan2 (QSumSinY, QSumCosY);
    QSumCosX:= 0;
    QSumSinX:= 0;
    QSumCosY:= 0;
    QSumSinY:= 0;
    QPoints:= 0;
  end;
end;

procedure RecalculateFrequencies;
// DDS Frequency and divider chain for Resonance Mode
var TwoPower28: extended;
begin
  with FrmMain do
  begin
    TwoPower28:= round (Power (2, 28));
    DDS_FrequencyOut:= (DDS_FrequencyWord * DDS_XtalFrequency) / TwoPower28;
    Divider_T5_Out:= DDS_FrequencyOut / Divider_T5;
    ControlFrequency:= Divider_T5_Out / Divider_Final;
    ControlPeriod:= 1.0 / ControlFrequency;
    TbxDDS_FrequencyOut.text:=  format ('%11.4f',  [DDS_FrequencyOut]);
    TbxDivider_T5_Out.text:=    format ('%12.10f', [Divider_T5_Out]);
    TbxControlFrequency.text:=  format ('%12.10f', [ControlFrequency]);
    TbxControlPeriod.text:=     format ('%12.10f', [ControlPeriod]);
    SendFrequencyWord:= true;
  end;
end;

procedure RecalculateAmplitudeSettings;
// recalc settings for Automatic Amplitude Control
var r: real;
begin
  with Frmmain do case AmplitudeControlMode of
    AmplitudeControlByRim_Mag:
    begin
      PeriodTime_Mag_ticks:= 40000; // Initial value. Augmented when the pendulum runs.
      // we calculate a targetvalue for the RimPasses belonging to the desired amplitude
      // See document "Parameters.doc, Fig 1."
      r:= arcsin (RimCoilRadius_mm / SetPoint_Amplitude_mm);
      Setpoint_Amplitude_ticks:= round (r * PeriodTime_Mag_ticks / 2 / pi);
      TbxTargetAmplitudeRim_Mag_ticks.text:= format ('%5d', [Setpoint_Amplitude_ticks]);
    end;

    AmplitudeControlByCenter_Cap:
    begin
    // Calculation not yet impelemented.
    // Enter value in ticks in the Radius_mm field
    Setpoint_Amplitude_ticks:= round (CenterElectrodeRadius_mm);
    TbxTargetAmplitudeCenterWidth_Cap_ticks.text:= format ('%5d', [Setpoint_Amplitude_ticks]);
    end;

    AmplitudeControlByTBD:;
    AmplitudeControlByNone:;
  end;
end;

begin
// no unit initialisation code
end.

