unit vector;

//Notes:
// Will likely not detect heading at North or South pole because the magnetic field lines are parallel to G.
// Invert Ya,m and Za,m per Fig 8 AN3192.pdf

{$mode objfpc}

interface

uses
  Classes, SysUtils, FileUtil, LResources, Forms, Controls, Graphics, Dialogs,
  StdCtrls, Grids, ExtCtrls, Spin, ComCtrls, math
  , OpenGLContext, GL, GLU //3D graphics
  , dynmatrix, dynmatrixutils //Matrix math
  , LazFileUtils //required to extractfilenameonly for converting to csv files
  , strutils //Required for checking lines in conversion file.
  , lazopenglcontext
;

type

  { TVectorForm }

  TVectorForm = class(TForm)
    AccelOpenGLControl: TOpenGLControl;
    AltAzOpenGLControl: TOpenGLControl;
    AStringGrid: TStringGrid;
    BubbleOpenGLControl: TOpenGLControl;
    ExportMagneto: TCheckBox;
    HSMagneto: TCheckBox;
    HSShowRaw: TCheckBox;
    HSSelectView: TButton;
    HSView: TButton;
    GroupBox1: TGroupBox;
    HSCreateCSV: TButton;
    HSOffsetFilename: TLabeledEdit;
    HSRawFilename: TLabeledEdit;
    InitialErrorLabel: TLabel;
    HSSavedFileEntry: TLabeledEdit;
    Deadbandlabel: TLabel;
    MagCalInstructionsLabel: TLabel;
    Memo2: TMemo;
    MinMaxCheckBox: TCheckBox;
    HSRecords: TLabeledEdit;
    OpenDialog1: TOpenDialog;
    Wwarning: TLabel;
    MagnetoCheckBox: TCheckBox;
    ReverseCheckBox: TCheckBox;
    HardSoftRecordToggle: TToggleBox;
    M1OpenGLControl: TOpenGLControl;
    M2OpenGLControl: TOpenGLControl;
    Memo1: TMemo;
    MRollcompOpenGLControl: TOpenGLControl;
    MxPlusGL: TOpenGLControl;
    MyPlusGL: TOpenGLControl;
    MzPlusGL: TOpenGLControl;
    MxMinusGL: TOpenGLControl;
    MyMinusGL: TOpenGLControl;
    MzMinusGL: TOpenGLControl;
    MPitchcompOpenGLControl: TOpenGLControl;
    MRollPitchcompOpenGLControl: TOpenGLControl;
    HardSoftOpenGLControl: TOpenGLControl;
    ResetMagCalButton: TButton;
    GetMagCalButton: TButton;
    Label1: TLabel;
    Label2: TLabel;
    Label3: TLabel;
    MagXCalOpenGLControl: TOpenGLControl;
    MagYCalOpenGLControl: TOpenGLControl;
    MagZCalOpenGLControl: TOpenGLControl;
    SetMagCalButton: TButton;
    HardSoftTabSheet: TTabSheet;
    TiltCompPage: TTabSheet;
    HSSampleTimer: TTimer;
    XmatrixLabel: TLabel;
    WmatrixLabel: TLabel;
    YmatrixLabel: TLabel;
    Label4: TLabel;
    Label5: TLabel;
    LevelLabel: TLabel;
    AltAzLabel: TLabel;
    VectorPageControl: TPageControl;
    SetP1Button: TButton;
    SetP2Button: TButton;
    SetP3Button: TButton;
    SetP4Button: TButton;
    SetP5Button: TButton;
    SetP6Button: TButton;
    SmoothedCheckBox: TCheckBox;
    IdleTimer1: TIdleTimer;
    OpenGLVersion: TLabeledEdit;
    DeadbandSpinEdit: TSpinEdit;
    SampleTempButton: TButton;
    SampleMagButton: TButton;
    SampleAccelButton: TButton;
    TabSheet1: TTabSheet;
    TabSheet2: TTabSheet;
    TabSheet3: TTabSheet;
    Valtitude: TLabeledEdit;
    Vazimuth: TLabeledEdit;
    Vmgrid: TStringGrid;
    Vmonitor: TToggleBox;
    VSampleGroupBox: TGroupBox;
    WStringGrid: TStringGrid;
    XStringGrid: TStringGrid;
    YStringGrid: TStringGrid;
    procedure AccelOpenGLControlPaint(Sender: TObject);
    procedure AltAzOpenGLControlPaint(Sender: TObject);
    procedure BubbleOpenGLControlPaint(Sender: TObject);
    procedure CreateMagnetoClick(Sender: TObject);
    procedure GetMagCalButtonClick(Sender: TObject);
    procedure HardSoftOpenGLControlMouseDown(Sender: TObject;
      Button: TMouseButton; Shift: TShiftState; X, Y: Integer);
    procedure HardSoftOpenGLControlMouseMove(Sender: TObject;
      Shift: TShiftState; X, Y: Integer);
    procedure HardSoftOpenGLControlPaint(Sender: TObject);
    procedure HardSoftRecordToggleChange(Sender: TObject);
    procedure HSCreateCSVClick(Sender: TObject);
    procedure HSSampleTimerTimer(Sender: TObject);
    procedure HSSelectViewClick(Sender: TObject);
    procedure HSShowRawClick(Sender: TObject);
    procedure HSViewClick(Sender: TObject);
    procedure IdleTimer1Timer(Sender: TObject);
    procedure MagXCalOpenGLControlPaint(Sender: TObject);
    procedure MagYCalOpenGLControlPaint(Sender: TObject);
    procedure MagZCalOpenGLControlPaint(Sender: TObject);
    procedure MinMaxCheckBoxChange(Sender: TObject);
    procedure MxMinusGLPaint(Sender: TObject);
    procedure MxPlusGLPaint(Sender: TObject);
    procedure MyMinusGLPaint(Sender: TObject);
    procedure MyPlusGLPaint(Sender: TObject);
    procedure MzMinusGLPaint(Sender: TObject);
    procedure MzPlusGLPaint(Sender: TObject);
    procedure ResetMagCalButtonClick(Sender: TObject);
    procedure SampleTempButtonClick(Sender: TObject);
    procedure SampleMagButtonClick(Sender: TObject);
    procedure SampleAccelButtonClick(Sender: TObject);
    procedure FormCreate(Sender: TObject);
    procedure FormShow(Sender: TObject);
    procedure SetMagCalButtonClick(Sender: TObject);
    procedure StartXRotationButtonClick(Sender: TObject);
    procedure StartYRotationButtonClick(Sender: TObject);
    procedure VectorPageControlChange(Sender: TObject);
    procedure VmonitorChange(Sender: TObject);
    procedure SetP5ButtonClick(Sender: TObject);
    procedure SetP6ButtonClick(Sender: TObject);
    procedure SetP3ButtonClick(Sender: TObject);
    procedure SetP4ButtonClick(Sender: TObject);
    procedure SetP1ButtonClick(Sender: TObject);
    procedure SetP2ButtonClick(Sender: TObject);
  private
    { private declarations }
    procedure AccelPlot();
    procedure MagPlot();
    procedure AltAzPlot();
    procedure DisplayAccelWXMatrix();
    procedure SetAccelCal(ACCpos:Integer);
    procedure DisplayMagCal();
    procedure MagCalPaint(MagWindow:TOpenGLControl; X,Y,Z,Bar,Max,Min:Double);
    procedure OpenGlwindowInit(Window: TOpenGLControl);
    procedure CalcStandard();
    procedure DisplayAzimuthComputations();
    procedure DisplayHSArray(X,Y:Integer);
  public
    { public declarations }
    procedure StopMonitoring();

  end;


//// Used to pass information to the conversion of raw values to final values
//type TAccMag = record
//		AX,AY, AZ, MX, MY, MZ : Double;
//	end;
//
//// Used to pass the final converted values for display and logging
//type TAltAz = record
//		Alt, Az : Double;
//	end;


//Accelerometer procedures
procedure GetAccel();
procedure GetAccelCal();
procedure ParseAccelCal(result:string);
procedure CalcAccelCal();
procedure NormalizeAccel();
function ComputeAltitude(X,Y,Z:float):float;

//Magnetometer procedures
procedure GetMag(Hide:Boolean=False);
procedure GetMagCal();
procedure NormalizeMag();
procedure ParseMagCal(result:string);
procedure ComputeAzimuth();

//Graphical procedures
procedure ArrowHeadProc2();
procedure DrawSphere(NumMajor, NumMinor: Integer; Radius: Double);
procedure textlabel(text:String);
procedure cube(X,Y,Z: Double);
//procedure dualcube(X1,Y1,Z1,X2,Y2,Z2: Double);
procedure AddMagPoint();

//Used to store a record of 3D integers
type TInt3D = record
 i1: Integer;
 i2: Integer;
 i3: Integer;
end;
type TFloat3D = record
 i1: Double;
 i2: Double;
 i3: Double;
end;

const
  smoothbuffersize = 5;//5 was OK
  MagScaleMax = 3200; //Raw magnetometer maximum
  MagZdepth = -3; //Z axis OpenGL display position
var
  MagArray: Array of TInt3d; //raw magnetometer readings array used for HS display
  M1Array: Array of TFloat3d; //Normalized magnetometer readings array used for HS display
  VectorForm: TVectorForm;
  AccCalXU, AccCalYU,AccCalZU,AccCalXD,AccCalYD,AccCalZD: Integer; //Accelerometer calibration Up/Down
  Axb,Ayb,Azb: Array[0..smoothbuffersize-1] of Double; //Smoothing buffer for accelerometer readings
  Ax,Ay,Az: Double; //Accelerometer raw (or smoothed) readings from meter
  Ax1, Ay1, Az1: Double;  //Accelerometer normalized deadbanded readings
  Ax1last, Ay1last, Az1last: Double;  //Accelerometer normalized deadbanded last readings
  GotAccCal, GotMagCal: Boolean;//Indicates that calibration data has been retrieved
  MinMaxMagCheck: Boolean;  //Checking if Magnetometer min max is being determined
  Mxmin: Double = 32767;
  Mymin: Double = 32767;
  Mzmin: Double = 32767;
  Mxmax: Double = -32767;
  Mymax: Double = -32767;
  Mzmax: Double = -32767;
  Mxb,Myb,Mzb: Array[0..smoothbuffersize-1] of Double; //Smoothing buffer for magnetometer readings
  Mx,My,Mz: Double; //Magnetic readings from meter
  MxOff,MyOff,MzOff: Double; //Magnetic reading raw offset
  Mx1, My1, Mz1: Double;  //Compass readings normalized
  Mxlast, Mylast, Mzlast: Double;  //Last compass readings for hard/soft iron plotting
  Mx2, My2, Mz2: Double;  //Compass readings corrected

  Cr1, Cx1, Cy1, Cz1: Double;  //Corrected once for roll (testing)
  Cr2, Cx2, Cy2, Cz2: Double;  //Corrected second time for pitch (testing)
  Mr1, MRx1, MRy1, MRz1: Double;  //Corrected once for roll (testing)
  Mr2, MRx2, MRy2, MRz2: Double;  //Corrected second time for pitch (testing)

  CXRot, CZRot: Double; //Correction roll and pitch
  MXRot, MZRot: Double; //Correction roll and pitch
  TXRot, TZRot: Double; //Target roll and pitch

  Pitch, Roll: Double;    //Calculated accelerometer values
  Pitch2, Roll2: Double;    //Calculated accelerometer values
  Pitch3, Roll3: Double;    //Calculated accelerometer values
  Y, w, X: TDMatrix;
  Ynorm: TDMatrix; //Normalized matrix of accelerometer values (Matrix Y)
  Wraw: TDMatrix; //Raw matrix of accelerometer values (Matrix w)

  //for alternate calibration based on magneto 1.2
  MAinv:TDMatrix;
  Mh:TDMatrix;
  Mb:TDMatrix;
  MhCal:TDMatrix;

  M: TDMatrix; //Multiplier matrix of accelerometer calibration values
  B: TDMatrix; //Offset matrix of accelerometer calibration values

  Mnorm: TDMatrix; //Normalized matrix of magnetometer values

  Heading: Double;//Magnetic heading

  srcblend, dstblend: GLenum;
  YellowLoop,ArrowHead: GLuint;//display list item

  HardSoft: Boolean; //inidicator that hard soft iron testing is being logged
  HSSample: Boolean; //Request to make a sample (at a slower rate than raw sampling)
  HSRecordCount: Integer; //Number of records written during Hard/Soft test.

  MonitorHidesSendGet : Boolean;
  CloseDLRecFile : Boolean; //Request to close the DL record file made by hard/soft recording

  XMouseOffset, YMouseOffset: Integer; //Used for Mouse movement like rotating GL objects
  XRotate, YRotate:Integer; //Used for Mouse movement like rotating GL objects
implementation
uses
  Unit1,header_utils,
  logcont, // for log one reading
  uglyfont; //OpenGL text

//const
//ACMmax = 32767;   //Accelerator Compass Module maximum value

{ TVectorForm }

procedure TVectorForm.VmonitorChange(Sender: TObject);
begin
  IdleTimer1.Enabled:=Vmonitor.Checked;
  VSampleGroupBox.Enabled:=not Vmonitor.Checked;
  MonitorHidesSendGet:=Vmonitor.Checked;
  if Vmonitor.Checked then begin//Initially gather calibration data
    GetAccelCal();
    DisplayAccelWXMatrix();
    StatusMessage('Vector monitoring mode, status messages suppressed.');
  end;
end;

procedure TVectorForm.SetP1ButtonClick(Sender: TObject);
begin
  SetAccelCal(1);
end;
procedure TVectorForm.SetP2ButtonClick(Sender: TObject);
begin
  SetAccelCal(2);
end;

procedure TVectorForm.SetP3ButtonClick(Sender: TObject);
begin
  SetAccelCal(3);
end;
procedure TVectorForm.SetP4ButtonClick(Sender: TObject);
begin
  SetAccelCal(4);
end;
procedure TVectorForm.SetP5ButtonClick(Sender: TObject);
begin
  SetAccelCal(5);
end;
procedure TVectorForm.SetP6ButtonClick(Sender: TObject);
begin
  SetAccelCal(6);
end;

//Initialize this page
procedure TVectorForm.FormCreate(Sender: TObject);
begin

  //No Hard/Soft iron compensation yet
  //VectorPageControl.Page[4].TabVisible:=False;

  SetLength(MagArray,1); //Array used for HS display
  SetLength(M1Array,1); //Array used for HS display

  OpenGlwindowInit(MRollcompOpenGLControl);
  textlabel('M_2');
  cube(Mx2,My2,Mz2);
  MRollcompOpenGLControl.SwapBuffers;

   //Create matrices
   Y:=Mzeros(6,3);
   w:=Mzeros(6,4);
   X:=Mzeros(4,3);
   Ynorm:=Mzeros(3,1);
   Wraw:=Mzeros(3,1);
   M:=Mzeros(3,3);
   B:=Mzeros(3,1);
   Mnorm:=Mzeros(3,1);

   //Create matrices for alternate Magneto 1.2 calculation
   MAinv:=Mzeros(3,3);
   Mh:=Mzeros(3,1);
   Mb:=Mzeros(3,1);
   MhCal:=Mzeros(3,1);

   //Label Y matrix
   YStringGrid.Cells[0,0]:='Y';
   YStringGrid.Cells[1,0]:='x';
   YStringGrid.Cells[2,0]:='y';
   YStringGrid.Cells[3,0]:='z';
   YStringGrid.Cells[0,1]:='P1';
   YStringGrid.Cells[0,2]:='P2';
   YStringGrid.Cells[0,3]:='P3';
   YStringGrid.Cells[0,4]:='P4';
   YStringGrid.Cells[0,5]:='P5';
   YStringGrid.Cells[0,6]:='P6';
   YStringGrid.Height:=YStringGrid.GridHeight;
   YStringGrid.Width:=YStringGrid.GridWidth;

   //Fill Y Matrix
   Y:=StringToDMatrix('0.0 0.0 1.0; 0.0 0.0 -1.0; 0.0 1.0 0.0; 0.0 -1.0 0.0; 1.0 0.0 0.0; -1.0 0.0 0.0');
   DMatrixToGrid(YStringGrid, Y);

   //Determined by Magneto 1.2 from initial prototype
   Mb:=StringToDMatrix('76.553492; -233.215243; 215.155570');
   //MAinv:=StringToDMatrix('1.040172 -0.014276 0.004515;-0.014276 1.029999  0.021719;0.004515 0.021719  1.071945'); //before 20160604
   //MAinv:=StringToDMatrix('0.940916 -0.041717 0.008803;-0.041717 0.957441 -0.013520;0.008803 -0.013520 0.902146'); //20160604 stight out
   //MAinv:=StringToDMatrix('0.940916 -0.041717 0.008803;-0.041717 0.957441 -0.013520;0.008803 -0.013520 0.902146');//tanslated
   MAinv:=StringToDMatrix(' 0.99294 0.043052 -0.0091776 0.043052 0.9761 0.014188 -0.0091776 0.014188 1.0341');//ellipsoid_fit2magnetic_data

   //Label W Matrix
   WStringGrid.Cells[0,0]:='W';
   WStringGrid.Cells[1,0]:='x';
   WStringGrid.Cells[2,0]:='y';
   WStringGrid.Cells[3,0]:='z';
   WStringGrid.Cells[0,1]:='P1';
   WStringGrid.Cells[0,2]:='P2';
   WStringGrid.Cells[0,3]:='P3';
   WStringGrid.Cells[0,4]:='P4';
   WStringGrid.Cells[0,5]:='P5';
   WStringGrid.Cells[0,6]:='P6';
   WStringGrid.Height:=WStringGrid.GridHeight;
   WStringGrid.Width:=WStringGrid.GridWidth;

   //label Accelerometer readings
   AStringGrid.Cells[0,0]:='Accel';
   AStringGrid.Cells[1,0]:='X';
   AStringGrid.Cells[2,0]:='Y';
   AStringGrid.Cells[3,0]:='Z';
   AStringGrid.Cells[0,1]:='raw';
   AStringGrid.Cells[0,2]:='norm';
   AStringGrid.Height:=AStringGrid.GridHeight;
   AStringGrid.Width:=AStringGrid.GridWidth;

   //Label X Matrix
   XStringGrid.Cells[0,1]:='A_1';
   XStringGrid.Cells[0,2]:='A_2';
   XStringGrid.Cells[0,3]:='A_3';
   XStringGrid.Cells[0,4]:='A_0';
   XStringGrid.Cells[1,0]:='A1_';
   XStringGrid.Cells[2,0]:='A2_';
   XStringGrid.Cells[3,0]:='A3_';
   XStringGrid.Height:=XStringGrid.GridHeight;
   XStringGrid.Width:=XStringGrid.GridWidth;

   //Label magnetic grid
   Vmgrid.Cells[0,0]:='Magnet';
   Vmgrid.Cells[0,1]:='Stored Max';
   Vmgrid.Cells[0,2]:='Max';
   Vmgrid.Cells[0,3]:='M_';
   Vmgrid.Cells[0,4]:='Min';
   Vmgrid.Cells[0,5]:='Stored Min';
   Vmgrid.Cells[0,6]:='Offset';
   Vmgrid.Cells[0,7]:='M_1(norm)';
   Vmgrid.Cells[0,8]:='M_2(corr)';
   Vmgrid.Cells[1,0]:='X axis';
   Vmgrid.Cells[2,0]:='Y axis';
   Vmgrid.Cells[3,0]:='Z axis';
   Vmgrid.Height:=Vmgrid.GridHeight;
   Vmgrid.Width:=Vmgrid.GridWidth;

   //Set to overview page initially
   VectorPageControl.ActivePageIndex:=0;
   VectorPageControlChange(nil);

end;

procedure TVectorForm.FormShow(Sender: TObject);
begin
  { TODO : make sure that these do not get activated when unminimized }
  GetAccelCal();
  GetMagCal();
  DisplayMagCal();
  //AccelPlot();
  //AltAzPlot();
  OpenGLVersion.Text:=glGetString(GL_VERSION);
end;

procedure TVectorForm.SampleTempButtonClick(Sender: TObject);
begin
  SendGet('v0x');
end;

procedure TVectorForm.GetMagCalButtonClick(Sender: TObject);
begin
  GetMagCal();
  DisplayMagCal();
end;

//Start rotating
procedure TVectorForm.HardSoftOpenGLControlMouseDown(Sender: TObject;
  Button: TMouseButton; Shift: TShiftState; X, Y: Integer);
begin
  XMouseOffset:=X;
  YMouseOffset:=Y;
end;

//Rotate
procedure TVectorForm.HardSoftOpenGLControlMouseMove(Sender: TObject;
  Shift: TShiftState; X, Y: Integer);
begin
  if (ssLeft in Shift) then begin
    XRotate:= XRotate + (Y-YMouseOffset);
    YRotate:= YRotate + (X-XMouseOffset);
    DisplayHSArray(XRotate,YRotate);
    XMouseOffset:=X;
    YMouseOffset:=Y;
  end;
end;

procedure TVectorForm.HardSoftOpenGLControlPaint(Sender: TObject);
begin
  //OpenGlwindowInit(HardSoftOpenGLControl);
  HardSoftOpenGLControl.SwapBuffers;
end;

// Hard/Soft Iron testing which includes logging
//  - the actual logging is done in the timer call.
procedure TVectorForm.HardSoftRecordToggleChange(Sender: TObject);
begin
  //Prepare for logging hard/soft iron data
  HardSoft:=HardSoftRecordToggle.Checked;

  //Initiate logging of data
  Vmonitor.Checked:=HardSoftRecordToggle.Checked;
  If HardSoftRecordToggle.Checked then begin
     WriteDLHeader('DL-V-HSLog', 'Hard/Soft calibration logging');
     HSSavedFileEntry.Text:=LogFileName;

     HSRecordCount:=0;//reset record count
     OpenGlwindowInit(HardSoftOpenGLControl);
     textlabel('Hard/Soft Iron test');
     HardSoftOpenGLControl.SwapBuffers;
     //dualcube(Ax1,Ay1,Az1,Mx1,My1,Mz1);
     //HardSoftOpenGLControl.SwapBuffers;

     //X +/- side display
     OpenGlwindowInit(MxPlusGL);
     textlabel('X +');
     MxPlusGL.SwapBuffers;
     OpenGlwindowInit(MxMinusGL);
     textlabel('X -');
     MxMinusGL.SwapBuffers;
     //Y +/- side display
     OpenGlwindowInit(MyPlusGL);
     textlabel('Y +');
     MyPlusGL.SwapBuffers;
     OpenGlwindowInit(MyMinusGL);
     textlabel('Y -');
     MyMinusGL.SwapBuffers;
     //Z +/- side display
     OpenGlwindowInit(MzPlusGL);
     textlabel('Z +');
     MzPlusGL.SwapBuffers;
     OpenGlwindowInit(MzMinusGL);
     textlabel('Z -');
     MzMinusGL.SwapBuffers;

     VmonitorChange(Nil);
     HSSampleTimer.Enabled:=True;
    end
  else begin
   HSSampleTimer.Enabled:=False;
   CloseDLRecFile:=True;
   HSView.Enabled:=True;
  end;
end;

//Create CSV file from saved random rotations
procedure TVectorForm.HSCreateCSVClick(Sender: TObject);
var
  RawCSVFilename, OffsetCSVFilename, FilenamePath: String;
  InFile,OutFile1,OutFile2: TextFile;
  Str: String;
  LogFileNameText, ExtensionText: String;
  pieces: TStringList;
  Mx,My,Mz: Float;

begin
  //Prepare filenames
  LogFileNameText:=HSSavedFileEntry.Text;
  FilenamePath:=ExtractFilePath(LogFileNameText);
  if ExportMagneto.Checked then
    ExtensionText:='.txt'
  else
    ExtensionText:='.csv';
  RawCSVFilename:=FilenamePath+'raw_'+ExtractFileNameOnly(LogFileNameText)+ExtensionText;
  OffsetCSVFilename:=FilenamePath+'off_'+ExtractFileNameOnly(LogFileNameText)+ExtensionText;
  HSRawFilename.Text:=RawCSVFilename;
  HSOffsetFilename.Text:=OffsetCSVFilename;

  //Convert from .dat to .csv files
  pieces := TStringList.Create;
  //Start reading file.
  AssignFile(InFile, LogFileNameText);
  AssignFile(OutFile1, RawCSVFilename);
  AssignFile(OutFile2, OffsetCSVFilename);
  {$I+}
  try
    Reset(InFile);

    //Open files for writing
    Rewrite(OutFile1);
    Rewrite(OutFile2);

    //output file header
    if not ExportMagneto.Checked then begin //magneto file has no header
      writeln(OutFile1,'X,Y,Z');
      writeln(OutFile2,'X,Y,Z');
    end;

    repeat
      // Read one line at a time from the file.
      Readln(InFile, Str);

      //Ignore comment lines which have # as first character.
      if not AnsiStartsStr('#',Str) then begin
          //Separate the fields of the record.
          pieces.Delimiter := ';';
          pieces.DelimitedText := Str;

          //Make sure there are enough fields
          if (pieces.Count<>6) then begin
              MessageDlg('Error', 'Incorrect number of fields in record.', mtError, [mbOK],0);
              break;
            end
          else begin

          //parse the fields, and convert as necessary.
          Mx:=StrToFloat(pieces.Strings[0]);
          My:=StrToFloat(pieces.Strings[1]);
          Mz:=StrToFloat(pieces.Strings[2]);
          Mx1:=StrToFloat(pieces.Strings[3]);
          My1:=StrToFloat(pieces.Strings[4]);
          Mz1:=StrToFloat(pieces.Strings[5]);

          if ExportMagneto.Checked then begin //magneto file uses space delimeter
            WriteLn(OutFile1,format('%4.1f %4.1f %4.1f',[Mx,My,Mz])); //Raw readings
            WriteLn(OutFile2,format('%4.1f %4.1f %4.1f',[Mx1,My1,Mz1])); //Normalized value
          end
          else begin //regular csv uses comma delimeter
            WriteLn(OutFile1,format('%4.1f,%4.1f,%4.1f',[Mx,My,Mz])); //Raw readings
            WriteLn(OutFile2,format('%4.1f,%4.1f,%4.1f',[Mx1,My1,Mz1])); //Normalized value
          end;



           end;//End of checking number of fields in record.

        end;
    until(EOF(InFile)); // EOF(End Of File) The the program will keep reading new lines until there is none.
    CloseFile(InFile);
  except
    on E: EInOutError do
    begin
     MessageDlg('Error', 'File handling error occurred. Details: '+E.ClassName+'/'+E.Message, mtError, [mbOK],0);
    end;
  end;
  Flush(OutFile1);
  CloseFile(OutFile1);
  Flush(OutFile2);
  CloseFile(OutFile2);

end;

procedure TVectorForm.HSSampleTimerTimer(Sender: TObject);
begin
  //Request to make a sample (at a slower rate than raw sampling)
  HSSample:=True;
end;

procedure TVectorForm.HSSelectViewClick(Sender: TObject);
begin
  if OpenDialog1.Execute then begin
      LogFileName:=OpenDialog1.Filename;
      HSSavedFileEntry.Text:=LogFileName;
    HSViewClick(Sender);
  end;
end;

procedure TVectorForm.HSShowRawClick(Sender: TObject);
begin
  DisplayHSArray(0,0);
end;

//View saved file
procedure TVectorForm.HSViewClick(Sender: TObject);
var
  InFile: TextFile;
  Str: String;
  pieces: TStringList;

begin
  Setlength(MagArray,1); // reset array size
  Setlength(M1Array,1); // reset array size

  //Convert from .dat to .csv files
  pieces := TStringList.Create;
  //Start reading file.
  AssignFile(InFile, HSSavedFileEntry.Text);
  {$I+}
  try
    Reset(InFile);

    repeat
      // Read one line at a time from the file.
      Readln(InFile, Str);

      //Ignore comment lines which have # as first character.
      if not AnsiStartsStr('#',Str) then begin
          //Separate the fields of the record.
          pieces.Delimiter := ';';
          pieces.DelimitedText := Str;

          //Make sure there are enough fields
          if (pieces.Count<>6) then begin
              MessageDlg('Error', 'Incorrect number of fields in record.', mtError, [mbOK],0);
              break;
            end
          else begin

          //parse the M_ (raw) fields, and store in memory
          Setlength(MagArray,Length(MagArray)+1); // Add one to array tomake room for new entries
          MagArray[Length(MagArray)-1].i1:=StrToInt(pieces.Strings[0]);
          MagArray[Length(MagArray)-1].i2:=StrToInt(pieces.Strings[1]);
          MagArray[Length(MagArray)-1].i3:=StrToInt(pieces.Strings[2]);

          //parse the M_1 (normalized) fields, and store in memory
          Setlength(M1Array,Length(M1Array)+1); // Add one to array tomake room for new entries
          //TODO: put into matrix for soft-iron compensation
          if HSMagneto.Checked then begin
            try
              //Alternate calibrated calculation based on Magneto 1.2 values
              Mh.setv(0,0,StrToInt(pieces.Strings[0]));
              Mh.setv(1,0,StrToInt(pieces.Strings[1]));
              Mh.setv(2,0,StrToInt(pieces.Strings[2]));
              MhCal:=MAinv * (Mh - Mb);
              M1Array[Length(M1Array)-1].i1:=MhCal.getv(0,0)/3200;
              M1Array[Length(M1Array)-1].i2:=MhCal.getv(1,0)/3200;
              M1Array[Length(M1Array)-1].i3:=MhCal.getv(2,0)/3200;
            except
              StatusMessage('Magneto settings exception');
            end;
          end
          else begin
           M1Array[Length(M1Array)-1].i1:=StrToFloat(pieces.Strings[3]);
           M1Array[Length(M1Array)-1].i2:=StrToFloat(pieces.Strings[4]);
           M1Array[Length(M1Array)-1].i3:=StrToFloat(pieces.Strings[5]);
          end;

          end;//End of checking number of fields in record.

        end;
    until(EOF(InFile)); // EOF(End Of File) The the program will keep reading new lines until there is none.
    CloseFile(InFile);
  except
    on E: EInOutError do
    begin
     MessageDlg('Error', 'File handling error occurred. Details: '+E.ClassName+'/'+E.Message, mtError, [mbOK],0);
    end;
  end;

//Display data
//Prepare graphics window
OpenGlwindowInit(HardSoftOpenGLControl);
HardSoftOpenGLControl.SwapBuffers;

XRotate:= 0;
YRotate:= 0;

DisplayHSArray(0,0);

end;
//Display sampled reading
procedure TVectorForm.DisplayHSArray(X,Y:Integer);
var
   i: Integer; // loop counter
   Distort: double; //Length from center
begin
  HardSoftOpenGLControl.MakeCurrent();
  glClear(GL_COLOR_BUFFER_BIT or GL_DEPTH_BUFFER_BIT); //Clear old background
  //writeln('X0=',XMouseOffset,' X=',X,' YO=',YMouseOffset,' Y=', Y, ' XR=',XRotate, ' YR=',YRotate); //debugging only
  //draw a bounding box around
  glPushMatrix();
  glTranslatef(0,0,-4);
  glRotatef(X, 1, 0, 0); //X rotation
  glRotatef(Y, 0, 1, 0); //Y rotation
  glBegin(GL_LINES);
    glLineWidth(0.1);
    glColor3f(1, 1, 1); //white lines
    glvertex3f(-1,-1,-1); glvertex3f(+1,-1,-1);
    glvertex3f(+1,-1,-1); glvertex3f(+1,+1,-1);
    glvertex3f(+1,+1,-1); glvertex3f(-1,+1,-1);
    glvertex3f(-1,+1,-1); glvertex3f(-1,-1,-1);

    glvertex3f(-1,-1,+1); glvertex3f(+1,-1,+1);
    glvertex3f(+1,-1,+1); glvertex3f(+1,+1,+1);
    glvertex3f(+1,+1,+1); glvertex3f(-1,+1,+1);
    glvertex3f(-1,+1,+1); glvertex3f(-1,-1,+1);

    glvertex3f(-1,-1,+1); glvertex3f(-1,-1,-1);
    glvertex3f(+1,-1,+1); glvertex3f(+1,-1,-1);
    glvertex3f(+1,+1,+1); glvertex3f(+1,+1,-1);
    glvertex3f(-1,+1,+1); glvertex3f(-1,+1,-1);

  glend();
  glPopMatrix();

  for i:=0 to length(MagArray)-1 do begin
   glPushMatrix();
   glTranslatef(0,0,-4);
   glRotatef(X, 1, 0, 0); //X rotation
   glRotatef(Y, 0, 1, 0); //Y rotation
   glPointSize(5);
   glBegin(GL_POINTS);
     if HSShowRaw.Checked then begin //Show raw points
       glColor3f(0.5,0.5,1);
       glVertex3f(MagArray[i].i1/MagScaleMax,MagArray[i].i2/MagScaleMax,MagArray[i].i3/MagScaleMax);
     end;
     //Show normalized points
     //glColor3f(1,0.5,0.5);//fixed colour
     //colourize dependent on squashedness
     Distort:=sqrt(M1Array[i].i1**2 + M1Array[i].i2**2 + M1Array[i].i3**2);
     Distort:=(Distort-1)*10;//exaggerate length
     glColor3f(1, 0.5 * (1-Distort), 0.5 * (1+Distort));//fixed colour
     glVertex3f(M1Array[i].i1,M1Array[i].i2,M1Array[i].i3);
   glEnd();
   glPopMatrix();
  end;
  HardSoftOpenGLControl.SwapBuffers;
end;

procedure TVectorForm.AccelOpenGLControlPaint(Sender: TObject);
begin

  OpenGlwindowInit(AccelOpenGLControl);

  //Draw accelerometer X,Y,Z magnitudes
  textlabel('Accelerometer');
  cube(Ax1,Ay1,Az1);
  cube(Cx1,Cy1,Cz1);//debug
  cube(Cx2,Cy2,Cz2);//debug

  AccelOpenGLControl.SwapBuffers;

end;

procedure TVectorForm.AltAzOpenGLControlPaint(Sender: TObject);
begin
  OpenGlwindowInit(AltAzOpenGLControl);
  AltAzOpenGLControl.SwapBuffers;
end;

procedure TVectorForm.BubbleOpenGLControlPaint(Sender: TObject);
begin
  //This also paints initial control black instead of leaving it unpainted.
  OpenGlwindowInit(BubbleOpenGLControl);
  BubbleOpenGLControl.SwapBuffers;
end;

procedure TVectorForm.CreateMagnetoClick(Sender: TObject);
begin

end;

procedure TVectorForm.IdleTimer1Timer(Sender: TObject);
begin

  Application.ProcessMessages;//This might allow OS to catch up and not report ~"stopped processing".

  IdleTimer1.Enabled:=False;
  GetAccel(); //gather and compensate accelerometer readings
  GetMag(True);  //gather and compensate magnetometer readings (uses compensated accelerometer values)
  CalcStandard();
  ComputeAzimuth();
  DisplayAzimuthComputations();
  AccelPlot();
  MagPlot();
  AltAzPlot();
  MagCalPaint(MagXCalOpenGLControl,Mx1,My1,Mz1,Mx,Mxmax,Mxmin);
  MagCalPaint(MagYCalOpenGLControl,My1,Mz1,Mx1,My,Mymax,Mymin);
  MagCalPaint(MagZCalOpenGLControl,Mz1,Mx1,My1,Mz,Mzmax,Mzmin);
  if Vmonitor.Checked then
    IdleTimer1.Enabled:=True;

  if HardSoft then begin
         //Start logging
         //Log reading if A_1 is different than last because it will only change once motionless.
         if ((Ax1<>Ax1last) and (Ay1<>Ay1last) and (Az1<>Az1last)) then begin
                Ax1last:=Ax1;
                Ay1last:=Ay1;
                Az1last:=Az1;
                if (HSSample) then begin
                       //Log reading to file
                       AssignFile(DLRecFile, LogFileName);
                       Reset(DLRecFile);
                       Append(DLRecFile); //Open file for appending
                       writeln(DLRecFile,format('%.0f;%.0f;%.0f;%.4f;%.4f;%.4f',[Mx,My,Mz,Mx1,My1,Mz1]));
                       Flush(DLRecFile);

                       //Incrememt on-screen record counter
                       inc(HSRecordCount);
                       HSRecords.Text:=IntToStr(HSRecordCount);

                       //Main Display of sampled reading
                       HardSoftOpenGLControl.MakeCurrent();
                       //AddMagPoint();
                       glPushMatrix();
                       glTranslatef(0,0,-4);
                       glBegin(GL_LINES);
                         glLineWidth(0.1);
                         glColor3f(1, 1, 1); //white line
                         glvertex3f(Mxlast/MagScaleMax,Mylast/MagScaleMax,Mzlast/MagScaleMax);
                         glvertex3f(Mx/MagScaleMax,My/MagScaleMax,Mz/MagScaleMax);
                       glend();

                       glPointSize(10);
                       glBegin(GL_POINTS);
                         glColor3f(0.5,0.5,1);
                         glVertex3f(Mx/MagScaleMax,My/MagScaleMax,Mz/MagScaleMax);
                       glEnd();
                       glPopMatrix();

                       //Save last place to draw line
                       Mxlast:=Mx;
                       Mylast:=My;
                       Mzlast:=Mz;
                       HardSoftOpenGLControl.SwapBuffers;

                       //X sides
                       if Mx>=0 then begin
                           MxPlusGL.MakeCurrent();
                           glPointSize(10);
                           glBegin(GL_POINTS);
                             glColor3f(0.5,0.5,1);
                             glVertex3f(My/MagScaleMax,Mz/MagScaleMax,MagZdepth);
                           glEnd();
                           MxPlusGL.SwapBuffers;
                         end
                       else begin
                           MxMinusGL.MakeCurrent();
                           glPointSize(10);
                           glBegin(GL_POINTS);
                             glColor3f(0.5,0.5,1);
                             glVertex3f(My/MagScaleMax,Mz/MagScaleMax,MagZdepth);
                           glEnd();
                           MxMinusGL.SwapBuffers;
                       end;
                       //Y sides
                       if My>=0 then begin
                           MyPlusGL.MakeCurrent();
                           glPointSize(10);
                           glBegin(GL_POINTS);
                             glColor3f(0.5,0.5,1);
                             glVertex3f(Mx/MagScaleMax,Mz/MagScaleMax,MagZdepth);
                           glEnd();
                           MyPlusGL.SwapBuffers;
                         end
                       else begin
                           MyMinusGL.MakeCurrent();
                           glPointSize(10);
                           glBegin(GL_POINTS);
                             glColor3f(0.5,0.5,1);
                             glVertex3f(Mx/MagScaleMax,Mz/MagScaleMax,MagZdepth);
                           glEnd();
                           MyMinusGL.SwapBuffers;
                       end;
                       //Z sides
                       if Mz>=0 then begin
                           MzPlusGL.MakeCurrent();
                           glPointSize(10);
                           glBegin(GL_POINTS);
                             glColor3f(0.5,0.5,1);
                             glVertex3f(Mx/MagScaleMax,My/MagScaleMax,MagZdepth);
                           glEnd();
                           MzPlusGL.SwapBuffers;
                         end
                       else begin
                           MzMinusGL.MakeCurrent();
                           glPointSize(10);
                           glBegin(GL_POINTS);
                             glColor3f(0.5,0.5,1);
                             glVertex3f(Mx/MagScaleMax,My/MagScaleMax,MagZdepth);
                           glEnd();
                           MzMinusGL.SwapBuffers;
                       end;

                       HSSample:=False;
                  end;
           end;
    end;
  if CloseDLRecFile then begin
    CloseDLRecFile:= False;
    CloseFile(DLRecFile);
  end;

end;

procedure TVectorForm.MagCalPaint(MagWindow:TOpenGLControl; X,Y,Z,Bar,Max,Min:Double);
var
   i : Double;
begin

    OpenGlwindowInit(MagWindow);

    //Bubble level

    //Direction indicator
    glPushMatrix();
      glTranslatef(0,0,-4);
      glRotatef(radtodeg(arctan2(Y,X)),1,0,0);
      glRotatef(radtodeg(arctan2(Z,sqrt(X**2 + Y**2 ))),0,1,0);
      //glCallList(ArrowHead); //Calllist removed in OpenGL 4
      ArrowHeadProc2();
    glPopMatrix();

    //Draw a loop for arrowhead/tail to fit into :
    //glCallList(YellowLoop); //Calllist removed in OpenGL 4
    glPushMatrix();
    glTranslatef(0,0,-4);

    glColor4f(1,1,1,1); //White, was transparent (1,1,1,0.5).
    glLineWidth(5);//was 1.5, but hard to see from a distance.
      glBegin(GL_LINE_LOOP);
        i:=0;
        while (i<2*pi) do
          begin
            glVertex3f( sin(i)/1.9, cos(i)/1.9, 1);
            i:= i+0.1;
          end;
      glEnd();
    glPopMatrix();


    //Value bar graph
    glPushMatrix();
      glTranslatef(0,0,-4);
      glColor3f(1,1,0);
      glBegin(GL_QUADS);
        glVertex3f(1.4, Bar/3000, 0);
        glVertex3f(1.5, Bar/3000, 0);
        glVertex3f(1.5,0, 0.0);
        glVertex3f(1.4,0, 0.0);
      glEnd();
      //New Min/Max lines
      glBegin(GL_LINES);
        glVertex3f(1.4, Max/3000, 0);
        glVertex3f(1.55, Max/3000, 0);
        glVertex3f(1.4, Min/3000, 0);
        glVertex3f(1.55, Min/3000, 0);
      glEnd();
    glPopMatrix();

    MagWindow.SwapBuffers;

end;

procedure TVectorForm.MagXCalOpenGLControlPaint(Sender: TObject);
begin
  OpenGlwindowInit(MagXCalOpenGLControl);
  MagXCalOpenGLControl.SwapBuffers;
end;

procedure TVectorForm.MagYCalOpenGLControlPaint(Sender: TObject);
begin
  OpenGlwindowInit(MagYCalOpenGLControl);
  MagYCalOpenGLControl.SwapBuffers;
end;

procedure TVectorForm.MagZCalOpenGLControlPaint(Sender: TObject);
begin
  OpenGlwindowInit(MagZCalOpenGLControl);
  MagZCalOpenGLControl.SwapBuffers;

end;


procedure TVectorForm.MinMaxCheckBoxChange(Sender: TObject);
begin
  MinMaxMagCheck:=MinMaxCheckBox.Checked;
end;

procedure TVectorForm.MxMinusGLPaint(Sender: TObject);
begin
  OpenGlwindowInit(MxMinusGL);
  MxMinusGL.SwapBuffers;
end;

procedure TVectorForm.MxPlusGLPaint(Sender: TObject);
begin
  OpenGlwindowInit(MxPlusGL);
  MxPlusGL.SwapBuffers;
end;

procedure TVectorForm.MyMinusGLPaint(Sender: TObject);
begin
  OpenGlwindowInit(MyMinusGL);
  MyMinusGL.SwapBuffers;
end;

procedure TVectorForm.MyPlusGLPaint(Sender: TObject);
begin
  OpenGlwindowInit(MyPlusGL);
  MyPlusGL.SwapBuffers;
end;

procedure TVectorForm.MzMinusGLPaint(Sender: TObject);
begin
  OpenGlwindowInit(MzMinusGL);
  MzMinusGL.SwapBuffers;
end;

procedure TVectorForm.MzPlusGLPaint(Sender: TObject);
begin
  OpenGlwindowInit(MzPlusGL);
  MzPlusGL.SwapBuffers;
end;

//Reset magnetometer range readings
//Use reasonable values so that autoranging starts off looking decent.
//Could use +/-32767, but that is extreme and the min/max cal display takes too many hand rotations to adjust.
procedure TVectorForm.ResetMagCalButtonClick(Sender: TObject);
const
 MminReset=-2000;
 MmaxReset= 2000;
begin
  Mxmin:= MminReset;
  Mymin:= MminReset;
  Mzmin:= MminReset;
  Mxmax:= MmaxReset;
  Mymax:= MmaxReset;
  Mzmax:= MmaxReset;
end;

procedure TVectorForm.SampleMagButtonClick(Sender: TObject);
begin
  GetMag(False);
end;

procedure TVectorForm.SampleAccelButtonClick(Sender: TObject);
begin
  GetAccel();
end;

procedure ParseAccelCal(result:string);
var
   pieces: TStringList;
   AccCalPos: Integer; //Accelerometer position
begin
  pieces := TStringList.Create;
  pieces.Delimiter := ',';

  pieces.DelimitedText := result;
  if ((pieces.Count>=5) and (pieces.Strings[0]='v3')) then
    begin
         AccCalPos:=StrToInt(pieces.Strings[1]);
         w.setv(AccCalPos-1, 0, StrToFloatDef(pieces.Strings[2],0));  //x
         w.setv(AccCalPos-1, 1, StrToFloatDef(pieces.Strings[3],0));  //y
         w.setv(AccCalPos-1, 2, StrToFloatDef(pieces.Strings[4],0));  //z
         w.setv(AccCalPos-1, 3, 1.0);  //constant
    end
    else begin
        if pieces.Count>0 then
          StatusMessage('V3x get accelerometer calibration failed. Count='+IntToStr(pieces.Count)+'. Reply= '+pieces.Strings[0])
        else
          StatusMessage('V3x get accelerometer calibration failed. Count=0. Reply= nil');
    end;
  if Assigned(pieces) then FreeAndNil(pieces);
end;

procedure TVectorForm.DisplayAccelWXMatrix();
var
   AccCalPos: Integer; //Accelerometer position
begin
  //Fill W matrix
  for AccCalPos:=1 to 6 do begin
     WStringGrid.Cells[1,AccCalPos]:=format('%6.0f',[w.getv(AccCalPos-1, 0)]);
     WStringGrid.Cells[2,AccCalPos]:=format('%6.0f',[w.getv(AccCalPos-1, 1)]);
     WStringGrid.Cells[3,AccCalPos]:=format('%6.0f',[w.getv(AccCalPos-1, 2)]);
     WStringGrid.Cells[4,AccCalPos]:=format('%6.0f',[w.getv(AccCalPos-1, 3)]);
  end;
  //Fill X matrix
  DMatrixToGrid(XStringGrid, X);
end;

procedure TVectorForm.SetMagCalButtonClick(Sender: TObject);
Begin
    ParseMagCal(SendGet('v4,'+
                 format('%6.5d,%6.5d,%6.5d,%6.5d,%6.5d,%6.5d',
                 [round(Mxmin), round(Mymin), round(Mzmin), +
                  round(Mxmax), round(Mymax), round(Mzmax) ]) +
                 'x'));
end;

procedure TVectorForm.StartXRotationButtonClick(Sender: TObject);
begin

end;

procedure TVectorForm.StartYRotationButtonClick(Sender: TObject);
begin

end;

//Examine tab change for special functions
procedure TVectorForm.VectorPageControlChange(Sender: TObject);
var
  page:Integer;
begin

 //On Linux, the opengl widgets are properly disabled on hidden tabs,
 // but they are all enabled on the Mac.

  //Get active page for comparisons
  page:=VectorPageControl.ActivePageIndex;

  //Overview tab
  AltAzOpenGLControl.Visible:=(page=0);
  BubbleOpenGLControl.Visible:=(page=0);

  //Accelerometer tab (no opengl widgets to enable)

  //Magnetometer tab
  MagXCalOpenGLControl.Visible:=(page=2);
  MagYCalOpenGLControl.Visible:=(page=2);
  MagZCalOpenGLControl.Visible:=(page=2);
  MPitchcompOpenGLControl.Visible:=(page=2);
  MRollPitchcompOpenGLControl.Visible:=(page=2);
  MRollcompOpenGLControl.Visible:=(page=2);

  //Tilt Compensated tab
  AccelOpenGLControl.Visible:=(page=3);
  M1OpenGLControl.Visible:=(page=3);
  M2OpenGLControl.Visible:=(page=3);

  //Hard/Soft Iron test tab
  MxPlusGL.Visible:=(page=4);
  MyPlusGL.Visible:=(page=4);
  MzPlusGL.Visible:=(page=4);
  MxMinusGL.Visible:=(page=4);
  MyMinusGL.Visible:=(page=4);
  MzMinusGL.Visible:=(page=4);
  HardSoftOpenGLControl.Visible:=(page=4);

end;

procedure GetMagCal();
begin
  ParseMagCal(SendGet('v4x'));
  GotMagCal:=True;
end;

//Gather and parse magnetometer calibration values
procedure ParseMagCal(result:string);
var
   pieces: TStringList;
begin
  pieces := TStringList.Create;
  pieces.Delimiter := ',';

  pieces.DelimitedText := result;
  if ((pieces.Count=7) and (pieces.Strings[0]='v4')) then begin
         //Parse minimum and maximum values
         Mxmin:=StrToFloatDef(pieces.Strings[1],0);
         Mymin:=StrToFloatDef(pieces.Strings[2],0);
         Mzmin:=StrToFloatDef(pieces.Strings[3],0);
         Mxmax:=StrToFloatDef(pieces.Strings[4],0);
         Mymax:=StrToFloatDef(pieces.Strings[5],0);
         Mzmax:=StrToFloatDef(pieces.Strings[6],0);
    end;
    //else
    //    StatusMessage('V4x get magnetometer calibration failed. Count='+IntToStr(pieces.Count)+'. Reply= '+pieces.Strings[0]);
  if Assigned(pieces) then FreeAndNil(pieces);
end;

//Display magnetometer calibration values
procedure TVectorForm.DisplayMagCal();
begin
     //Fill display with minimum and maximum "Stored" values
     Vmgrid.Cells[1,1]:=format('%7.0f',[Mxmax]);
     Vmgrid.Cells[2,1]:=format('%7.0f',[Mymax]);
     Vmgrid.Cells[3,1]:=format('%7.0f',[Mzmax]);
     Vmgrid.Cells[1,5]:=format('%7.0f',[Mxmin]);
     Vmgrid.Cells[2,5]:=format('%7.0f',[Mymin]);
     Vmgrid.Cells[3,5]:=format('%7.0f',[Mzmin]);
end;


procedure GetAccelCal();
var
   AccCalPos: Integer; //Accelerometer position
begin
  for AccCalPos := 1 to 6 do
     ParseAccelCal(SendGet(format('v3,%1dx',[AccCalPos])));

  CalcAccelCal();
end;
procedure TVectorForm.SetAccelCal(ACCpos:Integer);
Begin
  ParseAccelCal(SendGet('v3,'+
               format('%1d,%6.5d,%6.5d,%6.5d',
               [ACCpos,round(Ax), round(Ay), round(Az)]) +
               'x'));
  CalcAccelCal();
  DisplayAccelWXMatrix();
end;

procedure TVectorForm.CalcStandard();
begin
  Heading:=radtodeg(arctan2(-1*Mz2,Mx2))+180;

  //Display results
  Vazimuth.Text:=format('%4.0f',[Heading]);
  //Valtitude.Text:= format('%4.1f°',[radtodeg(arcsin(-1.0*Ax1))]);
  //Valtitude.Text:= format('%4.1f°',[radtodeg(arctan2(-1.0*Ax1,abs(sqrt(Ay1**2+Az1**2))))]);
  Valtitude.Text:= format('%4.1f°',[ComputeAltitude(Ax1, Ay1, Az1)]);

  //Display M_2 values
  Vmgrid.Cells[1,8]:=format('%3.3f',[Mx2]);
  Vmgrid.Cells[2,8]:=format('%3.3f',[My2]);
  Vmgrid.Cells[3,8]:=format('%3.3f',[Mz2]);
end;

function ComputeAltitude(X,Y,Z:float):float;
begin
  result:= roundto(radtodeg(arctan2(-1.0*X,abs(sqrt(Y**2+Z**2)))), -1);
end;

procedure ComputeAzimuth();
begin
  // Deconstruct Acceleration axis two rotations (to get rotations from "normal")
  //Determine first stage (X axis rotation to Y, Z=0):
  CXRot:=arctan2(Az1,Ay1);
  TXRot:=CXRot;//Later this should be changed to the sequentially determined "Mroll"
  //Make a new Y and Z from roll correction
  Cr1:=sqrt(Ay1**2+Az1**2);//Radius (Length of the line viewed down X axis)
  Cy1:=Cr1*cos(TXRot - CXRot);
  Cz1:=Cr1*sin(TXRot - CXRot);//Z=0 after rotation about Y axis
  Cx1:=Ax1; //No change for first correction rotation

  //Determine second stage (Z rotation to X, Y=0)
  Cr2:=sqrt(Cx1**2+Cy1**2);//Radius (Length of the line)
  CZRot:=arctan2(Cy1,Cx1);
  TZRot:=CZRot;////Later this should be changed to the sequentially determined "Mpitch"
  Cy2:=Cr2*sin(TZRot - CZRot);//Y=0 after rotation about Z axis.
  Cx2:=Cr2*cos(TZRot - CZRot);
  Cz2:=Cz1; //No change to Z for second correction
  //====================================================

  // Deconstruct Magnetometer axis two rotations (to get rotations from "normal")
  //Determine first stage (X axis rotation to Y, Z=0):
  MXRot:=arctan2(Mz1,My1);
  TXRot:=MXRot;//Later this should be changed to the sequentially determined "Mroll"
  //Make a new Y and Z from roll correction
  Mr1:=sqrt(My1**2+Mz1**2);//Radius (Length of the line viewed down X axis)
  MRy1:=Mr1;
  MRz1:=0;//Z=0 after rotation about Y axis
  MRx1:=Mx1; //No change for first correction rotation

  //Determine second stage (Z rotation to X, Y=0)
  Mr2:=sqrt(MRx1**2+MRy1**2);//Radius (Length of the line)
  MZRot:=arctan2(MRy1,MRx1);
  TZRot:=MZRot;////Later this should be changed to the sequentially determined "Mpitch"
  MRy2:=0;//Y=0 after rotation about Z axis.
  MRx2:=Mr2;
  MRz2:=MRz1; //No change to Z for second correction
  //====================================================

  // === Compensate Magnetometer with accelerometer rotations ===
  //correct for -XRot
  Mz2:=sin(MXRot - CXRot);
  My2:=cos(MXRot - CXRot);
  //then correct for -ZRot
  Mr2:=My2;
  My2:=Mr2*cos(MZRot - CZRot);
  Mx2:=Mr2*sin(MZRot - CZRot);
end;

procedure TVectorForm.DisplayAzimuthComputations();
begin
  memo1.Clear;

  memo1.Append(format('Ax1,Ay1,Az1 = %1.3f %1.3f %1.3f',[Ax1,Ay1,Az1]));

  // Deconstruct Acceleration axis two rotations (to get rotations from "normal")
  //Determine first stage (X axis rotation to Y, Z=0):
  memo1.Append(format('CXRot: %4.1f',[radtodeg(CXRot)]));
  //Make a new Y and Z from roll correction
  memo1.Append(format('Cx1,Cy1,Cz1 = %1.3f %1.3f %1.3f',[Cx1,Cy1,Cz1]));

  //Determine second stage (Z rotation to X, Y=0)
  memo1.Append(format('CZRot: %4.1f',[radtodeg(CZRot)]));
  memo1.Append(format('Cx2,Cy2,Cz2 = %1.3f %1.3f %1.3f',[Cx2,Cy2,Cz2]));

  // Deconstruct Magnetometer axis two rotations (to get rotations from "normal")
  //Determine first stage (X axis rotation to Y, Z=0):
  memo1.Append(format('Mx1,My1,Mz1 = %1.3f %1.3f %1.3f',[Mx1,My1,Mz1]));
  memo1.Append(format('MXRot: %4.1f',[radtodeg(MXRot)]));
  //Make a new Y and Z from roll correction
  memo1.Append(format('Mr1: %1.3f',[Mr1]));
  memo1.Append(format('MRx1,MRy1,MRz1 = %1.3f %1.3f %1.3f',[MRx1,MRy1,MRz1]));

  //Determine second stage (Z rotation to X, Y=0)
  memo1.Append(format('MZRot: %4.1f',[radtodeg(MZRot)]));
  memo1.Append(format('MRx2,MRy2,MRz2 = %1.3f %1.3f %1.3f',[MRx2,MRy2,MRz2]));

  // === Compensate Magnetometer with accelerometer rotations ===
  //correct for -XRot
  memo1.Append(format('1st Xrot: %4.1f ',[radtodeg(MXRot - CXRot)]));
  memo1.Append(format('1st stage Mx2,My2,Mz2 = %1.3f %1.3f %1.3f',[Mx2,My2,Mz2]));

  //then correct for -ZRot
  memo1.Append(format('1st Zrot: %4.1f ',[radtodeg(MZRot - CZRot)]));
  memo1.Append(format('1st stage Mx2,My2,Mz2 = %1.3f %1.3f %1.3f',[Mx2,My2,Mz2]));

  memo1.Append(format('Heading z,x : %4.1f',[radtodeg(arctan2(-1*Mz2,Mx2))+180]));
end;

procedure TVectorForm.MagPlot();
var
  i: Double;

  procedure cube(X,Y,Z: Double);
  begin
    glPushMatrix();

      //Move drawing to be viewed from above and on an angle
      glTranslatef(0,-0.2,-4);
      glRotatef( 30.0, 1, 0, 0); //X rotation
      glRotatef(-30.0, 0, 1, 0); //Y rotation

      //Draw outer loops
      glLineWidth(0.6);
      glColor4f(1, 1, 1,0.5); //white
      glBegin(GL_LINE_LOOP);
              i:=0;
              while (i<2*pi) do
                    begin
                      glVertex3f( sin(i), 0, cos(i));
                      i:= i+0.1;
                    end;
      glEnd();
      glBegin(GL_LINE_LOOP);
              i:=0;
              while (i<2*pi) do
                    begin
                      glVertex3f( sin(i), cos(i), 0);
                      i:= i+0.001;
                    end;
      glEnd();
      glBegin(GL_LINE_LOOP);
              i:=0;
              while (i<2*pi) do
                    begin
                      glVertex3f( 0, sin(i), cos(i));
                      i:= i+0.1;
                    end;
      glEnd();

      //Draw text axis labels
      glPushMatrix();
        glLineWidth(1.5);
        glRotatef(180, 1, 0, 0);
        glColor4f(1,0,0,0.5); //Red
        glTextOut( 1.2,0,0, 0.15, 0.2, 0.2, 1, 'X');
        glColor4f(0,0,1,0.5); //Blue
        glTextOut( 0,0,-1.2, 0.15, 0.2, 0.2, 1, 'Y');
        glColor4f(0,1,0,0.5); //Green
        glTextOut( 0,-1.2,0, 0.15, 0.2, 0.2, 1, 'Z');
      glPopMatrix();

      //Grid axis lines
      glBegin(GL_LINES);
        glColor4f(1,0,0,0.3);//Red
        glVertex3f(0,0,0);
        glVertex3f(1.1,0,0);
        glColor4f(0,1,0,0.3);//Green
        glVertex3f(0,0,0);
        glVertex3f(0,1.1,0);
        glColor4f(0,0,1,0.3);//Blue
        glVertex3f(0,0,0);
        glVertex3f(0,0,1.1);
      glEnd();

      //Value
      glLineWidth(2);
      glBegin(GL_LINES);
        glColor4f(1,0.5,0.5,0.9); //Red
        glVertex3f(0, 0,0);
        glVertex3f(X, 0,0); // Left/right  value
        glColor4f(0.5, 1, 0.5,0.9); //Green
        glVertex3f(0, 0,0);
        glVertex3f( 0,Z,0); // Up/down Axis
        glColor4f(0.5,0.5,1,0.9); //Blue
        glVertex3f(0, 0,0);
        glVertex3f(0,0, Y); // In/out axis
        glColor4f(1, 1, 1,0.9); //White
        glVertex3f(0, 0,0);
        glVertex3f(X, Z, Y); //Combined vector
      glEnd();

      glPointSize(4);
      glBegin(GL_POINTS);
        glColor4f(1,0.5,0.5,0.9); //Red
        glVertex3f(X, 0,0); // Left/right  value
        glColor4f(0.5, 1, 0.5,0.9); //Green
        glVertex3f( 0,Z,0); // Up/down Axis
        glColor4f(0.5,0.5,1,0.9); //Blue
        glVertex3f(0,0, Y); // In/out axis
        glColor4f(1, 1, 1,0.9); //White
        glVertex3f(X, Z, Y); //Combined vector
      glEnd();

    glPopMatrix();
  end;

  procedure textlabel(text:String);
  begin
    glPushMatrix();
      glLineWidth(1.5);
      //glLineWidth(0.5);
      glColor3f(1, 1, 1); //white
      glTranslatef(0,0.7,-2);
      glRotatef(180, 1, 0, 0);
      glTextOut(0,0,0, 0.07, 0.1, 0.1, 1, text);
    glPopMatrix();
  end;

begin
  //Fill up string grid with result from meter
  Vmgrid.Cells[1,2]:=format('%7.0f',[Mxmax]);
  Vmgrid.Cells[2,2]:=format('%7.0f',[Mymax]);
  Vmgrid.Cells[3,2]:=format('%7.0f',[Mzmax]);

  Vmgrid.Cells[1,3]:=format('%7.0f',[Mx]);
  Vmgrid.Cells[2,3]:=format('%7.0f',[My]);
  Vmgrid.Cells[3,3]:=format('%7.0f',[Mz]);

  Vmgrid.Cells[1,4]:=format('%7.0f',[Mxmin]);
  Vmgrid.Cells[2,4]:=format('%7.0f',[Mymin]);
  Vmgrid.Cells[3,4]:=format('%7.0f',[Mzmin]);

  Vmgrid.Cells[1,6]:=format('%3.3f',[MxOff]);
  Vmgrid.Cells[2,6]:=format('%3.3f',[MyOff]);
  Vmgrid.Cells[3,6]:=format('%3.3f',[MzOff]);

  Vmgrid.Cells[1,7]:=format('%3.3f',[Mx1]);
  Vmgrid.Cells[2,7]:=format('%3.3f',[My1]);
  Vmgrid.Cells[3,7]:=format('%3.3f',[Mz1]);

  OpenGlwindowInit(M1OpenGLControl);
  textlabel('Mx1, My1, Mz1');
  cube(Mx1,My1,Mz1);
  M1OpenGLControl.SwapBuffers;

  OpenGlwindowInit(M2OpenGLControl);
  textlabel('Mx2, My2, Mz2');
  cube(Mx2,My2,Mz2);
  M2OpenGLControl.SwapBuffers;

  //=== Roll compensated magnetometer readings =====================
  OpenGlwindowInit(MRollcompOpenGLControl);
  textlabel('M Roll comp');
glPushMatrix();

  //Move drawing to be viewed from above and on an angle
  glTranslatef(0,-0.4,-4);
  glRotatef(-1.0*radtodeg(Roll), 1, 0, 0 ); //Roll rotation

  //Draw outer loops
  glLineWidth(0.6);
  glColor4f(1, 1, 1,0.5); //white
  glBegin(GL_LINE_LOOP);
          i:=0;
          while (i<2*pi) do
                begin
                  glVertex3f( sin(i), 0, cos(i));
                  i:= i+0.1;
                end;
  glEnd();
  glBegin(GL_LINE_LOOP);
          i:=0;
          while (i<2*pi) do
                begin
                  glVertex3f( sin(i), cos(i), 0);
                  i:= i+0.001;
                end;
  glEnd();
  glBegin(GL_LINE_LOOP);
          i:=0;
          while (i<2*pi) do
                begin
                  glVertex3f( 0, sin(i), cos(i));
                  i:= i+0.1;
                end;
  glEnd();

  //Draw text axis labels
  glPushMatrix();
    glLineWidth(1.5);
    glRotatef(180, 1, 0, 0);
    glColor4f(1,0,0,0.5); //Red
    glTextOut( 1.2,0,0, 0.15, 0.2, 0.2, 1, 'X');
    glColor4f(0,0,1,0.5); //Blue
    glTextOut( 0,0,-1.2, 0.15, 0.2, 0.2, 1, 'Y');
    glColor4f(0,1,0,0.5); //Green
    glTextOut( 0,-1.2,0, 0.15, 0.2, 0.2, 1, 'Z');
  glPopMatrix();

  //Grid axis lines
  glBegin(GL_LINES);
    glColor4f(1,0,0,0.3);//Red
    glVertex3f(0,0,0);
    glVertex3f(1.1,0,0);
    glColor4f(0,1,0,0.3);//Green
    glVertex3f(0,0,0);
    glVertex3f(0,1.1,0);
    glColor4f(0,0,1,0.3);//Blue
    glVertex3f(0,0,0);
    glVertex3f(0,0,1.1);
  glEnd();

  //Value
  glLineWidth(2);
  glBegin(GL_LINES);
    glColor4f(1,0.5,0.5,0.9); //Red
    glVertex3f(0, 0,0);
    glVertex3f(Mx1, 0,0); // Left/right  value
    glColor4f(0.5, 1, 0.5,0.9); //Green
    glVertex3f(0, 0,0);
    glVertex3f( 0,Mz1,0); // Up/down Axis
    glColor4f(0.5,0.5,1,0.9); //Blue
    glVertex3f(0, 0,0);
    glVertex3f(0,0,My1); // In/out axis
    glColor4f(1, 1, 1,0.9); //White
    glVertex3f(0, 0,0);
    glVertex3f(Mx1,Mz1,My1); //Combined vector
  glEnd();

  glPointSize(4);
  glBegin(GL_POINTS);
    glColor4f(1,0.5,0.5,0.9); //Red
    glVertex3f(Mx1, 0,0); // Left/right  value
    glColor4f(0.5, 1, 0.5,0.9); //Green
    glVertex3f( 0,Mz1,0); // Up/down Axis
    glColor4f(0.5,0.5,1,0.9); //Blue
    glVertex3f(0,0,My1); // In/out axis
    glColor4f(1, 1, 1,0.9); //White
    glVertex3f(Mx1,Mz1,My1); //Combined vector
  glEnd();

glPopMatrix();

  MRollcompOpenGLControl.SwapBuffers;
  //==================================================

  //=== Pitch compensated magnetometer readings =====================
  OpenGlwindowInit(MPitchcompOpenGLControl);
  textlabel('M Pitch comp');
glPushMatrix();

  //Move drawing to be viewed from above and on an angle
  glTranslatef(0,-0.4,-4);
  glRotatef(radtodeg(Pitch3), 0, 0, 1 ); //Pitch rotation

  //Draw outer loops
  glLineWidth(0.6);
  glColor4f(1, 1, 1,0.5); //white
  glBegin(GL_LINE_LOOP);
          i:=0;
          while (i<2*pi) do
                begin
                  glVertex3f( sin(i), 0, cos(i));
                  i:= i+0.1;
                end;
  glEnd();
  glBegin(GL_LINE_LOOP);
          i:=0;
          while (i<2*pi) do
                begin
                  glVertex3f( sin(i), cos(i), 0);
                  i:= i+0.001;
                end;
  glEnd();
  glBegin(GL_LINE_LOOP);
          i:=0;
          while (i<2*pi) do
                begin
                  glVertex3f( 0, sin(i), cos(i));
                  i:= i+0.1;
                end;
  glEnd();

  //Draw text axis labels
  glPushMatrix();
    glLineWidth(1.5);
    glRotatef(180, 1, 0, 0);
    glColor4f(1,0,0,0.5); //Red
    glTextOut( 1.2,0,0, 0.15, 0.2, 0.2, 1, 'X');
    glColor4f(0,0,1,0.5); //Blue
    glTextOut( 0,0,-1.2, 0.15, 0.2, 0.2, 1, 'Y');
    glColor4f(0,1,0,0.5); //Green
    glTextOut( 0,-1.2,0, 0.15, 0.2, 0.2, 1, 'Z');
  glPopMatrix();

  //Grid axis lines
  glBegin(GL_LINES);
    glColor4f(1,0,0,0.3);//Red
    glVertex3f(0,0,0);
    glVertex3f(1.1,0,0);
    glColor4f(0,1,0,0.3);//Green
    glVertex3f(0,0,0);
    glVertex3f(0,1.1,0);
    glColor4f(0,0,1,0.3);//Blue
    glVertex3f(0,0,0);
    glVertex3f(0,0,1.1);
  glEnd();

  //Value
  glLineWidth(2);
  glBegin(GL_LINES);
    glColor4f(1,0.5,0.5,0.9); //Red
    glVertex3f(0, 0,0);
    glVertex3f(Mx1, 0,0); // Left/right  value
    glColor4f(0.5, 1, 0.5,0.9); //Green
    glVertex3f(0, 0,0);
    glVertex3f( 0,Mz1,0); // Up/down Axis
    glColor4f(0.5,0.5,1,0.9); //Blue
    glVertex3f(0, 0,0);
    glVertex3f(0,0,My1); // In/out axis
    glColor4f(1, 1, 1,0.9); //White
    glVertex3f(0, 0,0);
    glVertex3f(Mx1,Mz1,My1); //Combined vector
  glEnd();

  glPointSize(4);
  glBegin(GL_POINTS);
    glColor4f(1,0.5,0.5,0.9); //Red
    glVertex3f(Mx1, 0,0); // Left/right  value
    glColor4f(0.5, 1, 0.5,0.9); //Green
    glVertex3f( 0,Mz1,0); // Up/down Axis
    glColor4f(0.5,0.5,1,0.9); //Blue
    glVertex3f(0,0,My1); // In/out axis
    glColor4f(1, 1, 1,0.9); //White
    glVertex3f(Mx1,Mz1,My1); //Combined vector
  glEnd();

glPopMatrix();

  MPitchcompOpenGLControl.SwapBuffers;
  //==================================================

  //=== Roll then Pitch compensated magnetometer readings =====================
  OpenGlwindowInit(MRollPitchcompOpenGLControl);
  textlabel('M Roll Pitch comp');
  glPushMatrix();

  //Move drawing to be viewed from above and on an angle
  glTranslatef(0,-0.4,-4);
  glRotatef(-1.0*radtodeg(Roll), 1, 0, 0 ); //Roll rotation
  glRotatef(radtodeg(Pitch), 0, 0, 1 ); //Pitch rotation

  //Draw outer loops
  glLineWidth(0.6);
  glColor4f(1, 1, 1,0.5); //white
  glBegin(GL_LINE_LOOP);
          i:=0;
          while (i<2*pi) do
                begin
                  glVertex3f( sin(i), 0, cos(i));
                  i:= i+0.1;
                end;
  glEnd();
  glBegin(GL_LINE_LOOP);
          i:=0;
          while (i<2*pi) do
                begin
                  glVertex3f( sin(i), cos(i), 0);
                  i:= i+0.001;
                end;
  glEnd();
  glBegin(GL_LINE_LOOP);
          i:=0;
          while (i<2*pi) do
                begin
                  glVertex3f( 0, sin(i), cos(i));
                  i:= i+0.1;
                end;
  glEnd();

  //Draw text axis labels
  glPushMatrix();
    glLineWidth(1.5);
    glRotatef(180, 1, 0, 0);
    glColor4f(1,0,0,0.5); //Red
    glTextOut( 1.2,0,0, 0.15, 0.2, 0.2, 1, 'X');
    glColor4f(0,0,1,0.5); //Blue
    glTextOut( 0,0,-1.2, 0.15, 0.2, 0.2, 1, 'Y');
    glColor4f(0,1,0,0.5); //Green
    glTextOut( 0,-1.2,0, 0.15, 0.2, 0.2, 1, 'Z');
  glPopMatrix();

  //Grid axis lines
  glBegin(GL_LINES);
    glColor4f(1,0,0,0.3);//Red
    glVertex3f(0,0,0);
    glVertex3f(1.1,0,0);
    glColor4f(0,1,0,0.3);//Green
    glVertex3f(0,0,0);
    glVertex3f(0,1.1,0);
    glColor4f(0,0,1,0.3);//Blue
    glVertex3f(0,0,0);
    glVertex3f(0,0,1.1);
  glEnd();

  //Value
  glLineWidth(2);
  glBegin(GL_LINES);
    glColor4f(1,0.5,0.5,0.9); //Red
    glVertex3f(0, 0,0);
    glVertex3f(Mx1, 0,0); // Left/right  value
    glColor4f(0.5, 1, 0.5,0.9); //Green
    glVertex3f(0, 0,0);
    glVertex3f( 0,Mz1,0); // Up/down Axis
    glColor4f(0.5,0.5,1,0.9); //Blue
    glVertex3f(0, 0,0);
    glVertex3f(0,0,My1); // In/out axis
    glColor4f(1, 1, 1,0.9); //White
    glVertex3f(0, 0,0);
    glVertex3f(Mx1,Mz1,My1); //Combined vector
  glEnd();

  glPointSize(4);
  glBegin(GL_POINTS);
    glColor4f(1,0.5,0.5,0.9); //Red
    glVertex3f(Mx1, 0,0); // Left/right  value
    glColor4f(0.5, 1, 0.5,0.9); //Green
    glVertex3f( 0,Mz1,0); // Up/down Axis
    glColor4f(0.5,0.5,1,0.9); //Blue
    glVertex3f(0,0,My1); // In/out axis
    glColor4f(1, 1, 1,0.9); //White
    glVertex3f(Mx1,Mz1,My1); //Combined vector
  glEnd();

  glPopMatrix();

  //Draw small circle from top view
  glPushMatrix();
    glLineWidth(0.6);
    glColor4f(1, 1, 1,0.9); //White
    glTranslatef(-1.5,1.5,-8);
    glBegin(GL_LINE_LOOP);
            i:=0;
            while (i<2*pi) do
                  begin
                    glVertex3f( sin(i), cos(i), 0);
                    i:= i+0.001;
                  end;
    glEnd();
    //Value
    glBegin(GL_LINES);
      glVertex3f(0, 0,0);
      glVertex3f(Mx1,My1,0); //Combined vector
    glEnd();
    glPointSize(4);
    glBegin(GL_POINTS);
      glVertex3f(Mx1,My1,0); //Combined vector
    glEnd();
  glPopMatrix();

  //Draw small circle from top view
  glPushMatrix();
    glLineWidth(0.6);
    glColor4f(1, 1, 1,0.9); //White
    glTranslatef(1.5,1.5,-8);
    glBegin(GL_LINE_LOOP);
            i:=0;
            while (i<2*pi) do
                  begin
                    glVertex3f( sin(i), cos(i), 0);
                    i:= i+0.001;
                  end;
    glEnd();
    //Value
    glBegin(GL_LINES);
      glVertex3f(0, 0,0);
      glVertex3f(Mx2,My2,0); //Combined vector
    glEnd();
    glPointSize(4);
    glBegin(GL_POINTS);
      glVertex3f(Mx2,My2,0); //Combined vector
    glEnd();
  glPopMatrix();

  MRollPitchcompOpenGLControl.SwapBuffers;
  //==================================================


end;

procedure TVectorForm.AccelPlot();
begin
  //display raw values
  AStringGrid.Cells[1,1]:=format('%7.0f',[Ax]);
  AStringGrid.Cells[2,1]:=format('%7.0f',[Ay]);
  AStringGrid.Cells[3,1]:=format('%7.0f',[Az]);

  //Enable Acceleration calibration buttons
  SetP5Button.Enabled:=Ax >  8000;
  SetP6Button.Enabled:=Ax < -8000;
  SetP3Button.Enabled:=Ay >  8000;
  SetP4Button.Enabled:=Ay < -8000;
  SetP1Button.Enabled:=Az >  8000;
  SetP2Button.Enabled:=Az < -8000;

  //Display normalized deadbanded values
  AStringGrid.Cells[1,2]:=format('%7.3f',[Ax1]);
  AStringGrid.Cells[2,2]:=format('%7.3f',[Ay1]);
  AStringGrid.Cells[3,2]:=format('%7.3f',[Az1]);

  AccelOpenGLControl.Paint;
end;

procedure TVectorForm.AltAzPlot();
var
  i: Double;

  procedure textlabel(text:String);
  begin
    glPushMatrix();
      glLineWidth(1.5);
      //glLineWidth(0.5);
      glColor3f(1, 1, 1); //white
      glTranslatef(0,0.7,-2);
      glRotatef(180, 1, 0, 0);
      glTextOut(0,0,0, 0.07, 0.1, 0.1, 1, text);
    glPopMatrix();
  end;

begin

  OpenGlwindowInit(AltAzOpenGLControl);

  //Draw final Altitude Azimuth drawing
  //textlabel('Alt Az');
  glPushMatrix();

    //Move drawing to be viewed from above and on an angle
    glTranslatef(0,-0.2,-4);
    glRotatef( 30.0, 1, 0, 0); //X rotation
    glRotatef(-30.0, 0, 1, 0); //Y rotation

    //Draw outer loops
    glLineWidth(0.6);
    glColor4f(1, 1, 1,0.5); //white
    glBegin(GL_LINE_LOOP);
            i:=0;
            while (i<2*pi) do
                  begin
                    glVertex3f( sin(i), 0, cos(i));
                    i:= i+0.1;
                  end;
    glEnd();
    glBegin(GL_LINES);
            i:=0;
            while (i<pi/2) do
                  begin
                    glVertex3f( sin(i), cos(i), 0);
                    i:= i+0.001;
                  end;
    glEnd();

    //Draw text axis labels
    glPushMatrix();
      glLineWidth(1.0);
      glRotatef(180, 1, 0, 0);
      glColor4f(1,1,1,0.5);
      glTextOut( 0,-1.2,0, 0.075, 0.1, 0.1, 1, 'Zenith');
    glPopMatrix();

    //Grid axis lines
    glBegin(GL_LINES);
      glColor4f(1,0,0,0.3);//Red
      glVertex3f(0,0,0);
      glVertex3f(1.1,0,0);
      glColor4f(0,1,0,0.3);//Green
      glVertex3f(0,0,0);
      glVertex3f(0,1.1,0);
    glEnd();

    //Altitude line
    if Ax1<0 then
      glColor4f(1,1,0,1) //Bright yellow
    else
      glColor4f(0.4,0.4,0,1); //Dim yellow
    glLineWidth(2);
    glBegin(GL_LINES);
      glVertex3f(0, 0,0);
      //glVertex3f(cos(arcsin(Ax1)),Ax1, 0); // Left/right  value (based on X only) too noisy near X-zero
      glVertex3f(sqrt(Ay1**2+Az1**2),-1*Ax1, 0); // Left/right  value (based on all three axis)
    glEnd();
    //Altitude dot
    glPointSize(4);
    glBegin(GL_POINTS);
      glVertex3f(sqrt(Ay1**2+Az1**2),-1*Ax1, 0); // Left/right  value (based on all three axis)
    glEnd();

    //Compass line pointer (to North)
    //glBegin(GL_LINES);
    //  glColor4f(0.5,1.0,0.5,0.9); //Green
    //  glVertex3f(0, 0,0);
    //  glVertex3f(sin(degtorad(Heading)), 0,-1.0*cos(degtorad(Heading))); // Left/right  value
    //glEnd();

    //Compass heading dial (rotating)
    glPushMatrix();
      glLineWidth(1.5);
      glColor4f(0,1,0,1); //Green
      glRotatef(90,1,0,0);
      glRotatef(90,0,0,1);
      glRotatef(-1*Heading, 0,0,1);//Correct direction
      glTextOut( 0, -1.2, 0, 0.15, 0.2, 0.2, 1, 'N');
      glTextOut( 1.2,  0, 0, 0.15, 0.2, 0.2, 1, 'E');
      glTextOut( -1.2, 0, 0, 0.15, 0.2, 0.2, 1, 'W');
      glTextOut( 0,  1.2, 0, 0.15, 0.2, 0.2, 1, 'S');
    glPopMatrix();
  glPopMatrix();
  AltAzOpenGLControl.SwapBuffers;


  //Bubble level
  OpenGlwindowInit(BubbleOpenGLControl);
  // -- Outer loop
  glPushMatrix();
    glTranslatef(0,0,-1);
    glColor4f(1,1,1,0.5); //light grey
    glLineWidth(1.5);
    glBegin(GL_LINE_LOOP);
      i:=0;
      while (i<2*pi) do
        begin
          glVertex3f( sin(i)/4, cos(i)/4, 0);
          i:= i+0.1;
        end;
    glEnd();
  glPopMatrix();
  // -- Moveable loop
  glPushMatrix();
    glTranslatef(Az1,-1*Ay1,-1);
    glColor4f(1,1,0,1); //Yellow
    glLineWidth(1.5);
    glBegin(GL_LINE_LOOP);
      i:=0;
      while (i<2*pi) do
        begin
          glVertex3f( sin(i)/5, cos(i)/5, 0);
          i:= i+0.1;
        end;
    glEnd();
  glPopMatrix();
  BubbleOpenGLControl.SwapBuffers;
end;


procedure TVectorForm.StopMonitoring();
begin
  Vmonitor.Checked:=False;
  IdleTimer1.Enabled:=False;
  VSampleGroupBox.Enabled:=True;
end;

//Calculate accelerometer calibration values
procedure CalcAccelCal();
begin
  //Calibrate accelerometer based on:
  //   Equation 20, pg 16, AN3182.pdf rev 1, for LIS331DLH 3-axis digital accelerometer
  try begin
    //Wwarning.Caption:='';
    X:= Minv(w.t * w) * w.t * Y; //Works fine if values are real calibrations.
  end;
  except begin
    //Wwarning.Caption:='Uncalibrated values in matrix!';
    X:= w.t * Y; //Junk just to get data before calibrations have been entered.
    end;
  end;
  GotAccCal:=True;
end;

//Generic clearing of a OpenGL window for this application.
//Called for each OpenGL window
procedure TVectorForm.OpenGlwindowInit(Window: TOpenGLControl);
begin
  Window.MakeCurrent();
  glEnable (GL_LINE_SMOOTH);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  glClearColor(0, 0, 0, 1.0); //Background
  glClear(GL_COLOR_BUFFER_BIT or GL_DEPTH_BUFFER_BIT);
  glEnable(GL_DEPTH_TEST);
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluPerspective(45.0, double(100) / 100, 0.1, 100.0);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

end;

procedure ArrowHeadProc2();
var
  i:Double;
begin

  glBegin(GL_TRIANGLE_FAN);
    glColor4f(1,1,0,1); //Yellow
    glVertex3f(0, 0,1);
    i:=-0.1;
    while (i<2*pi) do
      begin
        glVertex3f( sin(i)/2, cos(i)/2, 1);
        i:= i+0.1;
      end;
  glEnd();

  //Dot
  glBegin(GL_TRIANGLE_FAN);
    glColor4f(0,0,0,1); //Black
    glVertex3f(0, 0,1.01);
    i:=-0.1;
    while (i<2*pi) do
      begin
        glVertex3f( sin(i)/8, cos(i)/8, 1.01);
        i:= i+0.1;
      end;
  glEnd();

  glBegin(GL_TRIANGLE_FAN);
    glColor4f(0,1,0,1); //Green
    glVertex3f(0, 0,-1);
    i:=-0.1;
    while (i<2*pi) do
      begin
        glVertex3f( sin(i)/2, cos(i)/2, -1);
        i:= i+0.1;
      end;
  glEnd();

  //Cross
  glColor3f(0,0,0); //Black
  glBegin(GL_QUADS);
    glVertex3f(-0.08, -0.35, -1.01);
    glVertex3f(-0.08,  0.35, -1.01);
    glVertex3f( 0.08,  0.35, -1.01);
    glVertex3f( 0.08, -0.35, -1.01);
  glEnd();
  glBegin(GL_QUADS);
    glVertex3f(-0.35, -0.08, -1.01);
    glVertex3f( 0.35, -0.08, -1.01);
    glVertex3f( 0.35,  0.08, -1.01);
    glVertex3f(-0.35,  0.08, -1.01);
  glEnd();
  //Arrow feathers
  glBegin(GL_TRIANGLE_FAN);
    glColor3f(0.3,0.3,0.3); //grey
    glVertex3f(0,0,-0.5);
    glColor3f(0.4,0.4,0.4); //light grey
    glVertex3f(-0.08, -0.35, -1.01);
    glColor3f(0.3,0.3,0.3); //grey
    glVertex3f(-0.08,  0.35, -1.01);
    glColor3f(0.4,0.4,0.4); //light grey
    glVertex3f( 0.08,  0.35, -1.01);
    glColor3f(0.3,0.3,0.3); //grey
    glVertex3f( 0.08, -0.35, -1.01);
    glColor3f(0.4,0.4,0.4); //light grey
    glVertex3f(-0.08, -0.35, -1.01);
  glEnd();
  glBegin(GL_TRIANGLE_FAN);
    glColor3f(0.3,0.3,0.3); //grey
    glVertex3f(0,0,-0.5);
    glColor3f(0.4,0.4,0.4); //light grey
    glVertex3f(-0.35, -0.08, -1.01);
    glColor3f(0.3,0.3,0.3); //grey
    glVertex3f( 0.35, -0.08, -1.01);
    glVertex3f( 0.35,  0.08, -1.01);
    glColor3f(0.4,0.4,0.4); //light grey
    glVertex3f(-0.35,  0.08, -1.01);
    glColor3f(0.3,0.3,0.3); //grey
    glVertex3f(-0.35, -0.08, -1.01);
  glEnd();
  //Arrow tip
  glColor3f(0.3,0.3,0.3); //grey
  glBegin(GL_TRIANGLE_FAN);
    glVertex3f(0,0,1);
    i:=-0.1;
    while (i<2*pi) do
      begin
        glVertex3f( sin(i)/3, cos(i)/3, 0.5);
        i:= i+0.1;
      end;
  glEnd();
  //Arrow shaft
  glBegin(GL_TRIANGLE_STRIP);
    glVertex3f(0,0,1);
    i:=-0.1;
    while (i<2*pi) do
      begin
        glVertex3f( sin(i)/12, cos(i)/12, 0.6);
        glVertex3f( sin(i)/12, cos(i)/12, -1);
        i:= i+0.1;
      end;
  glEnd();

end;


procedure DrawSphere(NumMajor, NumMinor: Integer; Radius: Double);
var
  MajorStep, MinorStep: Double;
  i, j: Integer;
  a, b, c, r0, r1: Double;
  x, y, z0, z1: Double;
begin
  MajorStep := (PI / NumMajor);
  MinorStep  := (2.0 * PI / NumMinor);

   for i := 0 to NumMajor do
   begin
      a := i * MajorStep;
      b := a + MajorStep;
      r0 := radius * Sin(a);
      r1 := radius * Sin(b);
      z0 := radius * Cos(a);
      z1 := radius * Cos(b);

      glBegin(GL_TRIANGLE_STRIP);
      for j := 0 to NumMinor do
      begin
       	 c := j * MinorStep;
         x := Cos(c);
         y := Sin(c);

         glNormal3f((x * r0) / radius, (y * r0) / Radius, z0 / Radius);
         glTexCoord2f(j / NumMinor, i / NumMajor);
         glVertex3f(x * r0, y * r0, z0);

         glNormal3f((x * r1) / Radius, (y * r1) / Radius, z1 / Radius);
         glTexCoord2f(j / NumMinor, (i + 1) / NumMajor);
         glVertex3f(x * r1, y * r1, z1);
      end;
   glEnd();
   end;
end;

procedure Torus();
var
  ii,j,k:Integer;
  s,t,xt,yt,zt:Double;
const
  numc=100;
  numt=100;
  TWOPI = 2*pi;
begin
glPushMatrix();
  glTranslatef(0,0,-4);
  glColor4f(1,1,0,1); //Yellow
  for ii:= 0 to numc-1 do
     begin
      glBegin(GL_QUAD_STRIP);
      for j:= 0 to numt do
         begin
          for k:=1 downto -1 do
            begin
              s:= (ii + k) mod numc + 0.5;
              t:= j mod numt;

              xt:= (1 + 0.1 * Cos(s * TWOPI / numc)) * Cos(t * TWOPI / numt);
              yt:= (1 + 0.1 * Cos(s * TWOPI / numc)) * Sin(t * TWOPI / numt);
              zt:= 0.1 * Sin(s * TWOPI / numc);

              glVertex3d( xt,  yt,  zt);
          end;
      end;
      glEnd();
  end;
  glPopMatrix();
end;
procedure AddMagPoint();
begin
  glPushMatrix();
  glTranslatef(0,0,-4);
  glBegin(GL_LINES);
    glLineWidth(0.1);
    glColor3f(1, 1, 1); //white
    glvertex3f(Mxlast,Mylast,Mzlast);
    glvertex3f(Mx1,My1,Mz1);
  glend();

  glPointSize(10);
  glBegin(GL_POINTS);
    glColor3f(0.5,0.5,1);
    glVertex3f(Mx1,My1,Mz1);
  glEnd();

  glPopMatrix();
end;
procedure textlabel(text:String);
begin
  glPushMatrix();
    glLineWidth(1.5);
    glColor3f(1, 1, 1); //white
    glTranslatef(0,0.7,-2);
    glRotatef(180, 1, 0, 0);
    glTextOut(0,0,0, 0.07, 0.1, 0.1, 1, text);
  glPopMatrix();
end;

procedure cube(X,Y,Z: Double);
var
  i:Double;
begin
  glPushMatrix();

    //Move drawing to be viewed from above and on an angle
    glTranslatef(0,-0.2,-4);
    glRotatef( 30.0, 1, 0, 0); //X rotation
    glRotatef(-30.0, 0, 1, 0); //Y rotation

    //Draw outer loops
    glLineWidth(0.6);
    glColor4f(1, 1, 1,0.5); //white
    glBegin(GL_LINE_LOOP);
            i:=0;
            while (i<2*pi) do
                  begin
                    glVertex3f( sin(i), 0, cos(i));
                    i:= i+0.1;
                  end;
    glEnd();
    glBegin(GL_LINE_LOOP);
            i:=0;
            while (i<2*pi) do
                  begin
                    glVertex3f( sin(i), cos(i), 0);
                    i:= i+0.001;
                  end;
    glEnd();
    glBegin(GL_LINE_LOOP);
            i:=0;
            while (i<2*pi) do
                  begin
                    glVertex3f( 0, sin(i), cos(i));
                    i:= i+0.1;
                  end;
    glEnd();

    //Draw text axis labels
    glPushMatrix();
      glLineWidth(1.5);
      glRotatef(180, 1, 0, 0);
      glColor4f(1,0,0,0.5); //Red
      glTextOut( 1.2,0,0, 0.15, 0.2, 0.2, 1, 'X');
      glColor4f(0,0,1,0.5); //Blue
      glTextOut( 0,0,-1.2, 0.15, 0.2, 0.2, 1, 'Y');
      glColor4f(0,1,0,0.5); //Green
      glTextOut( 0,-1.2,0, 0.15, 0.2, 0.2, 1, 'Z');
    glPopMatrix();

    //Grid axis lines
    glBegin(GL_LINES);
      glColor4f(1,0,0,0.3);//Red
      glVertex3f(0,0,0);
      glVertex3f(1.1,0,0);
      glColor4f(0,1,0,0.3);//Green
      glVertex3f(0,0,0);
      glVertex3f(0,1.1,0);
      glColor4f(0,0,1,0.3);//Blue
      glVertex3f(0,0,0);
      glVertex3f(0,0,1.1);
    glEnd();

    //Value
    glLineWidth(2);
    glBegin(GL_LINES);
      glColor4f(1,0.5,0.5,0.9); //Red
      glVertex3f(0, 0,0);
      glVertex3f(X, 0,0); // Left/right  value
      glColor4f(0.5, 1, 0.5,0.9); //Green
      glVertex3f(0, 0,0);
      glVertex3f( 0,Z,0); // Up/down Axis
      glColor4f(0.5,0.5,1,0.9); //Blue
      glVertex3f(0, 0,0);
      glVertex3f(0,0, Y); // In/out axis
      glColor4f(1, 1, 1,0.9); //White
      glVertex3f(0, 0,0);
      glVertex3f(X, Z, Y); //Combined vector
    glEnd();

    glPointSize(4);
    glBegin(GL_POINTS);
      glColor4f(1,0.5,0.5,0.9); //Red
      glVertex3f(X, 0,0); // Left/right  value
      glColor4f(0.5, 1, 0.5,0.9); //Green
      glVertex3f( 0,Z,0); // Up/down Axis
      glColor4f(0.5,0.5,1,0.9); //Blue
      glVertex3f(0,0, Y); // In/out axis
      glColor4f(1, 1, 1,0.9); //White
      glVertex3f(X, Z, Y); //Combined vector
    glEnd();

  glPopMatrix();
end;
//Used to show tilt compensation
//procedure dualcube(X1,Y1,Z1,X2,Y2,Z2: Double);
//var
//  i:Double;
//const
//  LoopRadius = 1;
//begin
//
//  //writeln(format('Acc= %2.3f   Mag= %2.3f',[sqrt(X1**2+Y1**2+Z1**2),sqrt(X2**2+Y2**2+Z2**2)]));
//  glPushMatrix();
//
//    //Move drawing to be viewed from above and on an angle
//    glTranslatef(0,-0.2,-4);
//    glRotatef( 30.0, 1, 0, 0); //X rotation
//    glRotatef(-30.0, 0, 1, 0); //Y rotation
//
//    //Draw outer loops
//    glLineWidth(0.6);
//    glColor4f(1, 1, 1,0.5); //white
//    glBegin(GL_LINE_LOOP);
//            i:=0;
//            while (i<2*pi) do
//                  begin
//                    glVertex3f(LoopRadius* sin(i), 0, LoopRadius * cos(i));
//                    i:= i+0.1;
//                  end;
//    glEnd();
//    glBegin(GL_LINE_LOOP);
//            i:=0;
//            while (i<2*pi) do
//                  begin
//                    glVertex3f(LoopRadius *  sin(i),LoopRadius *  cos(i), 0);
//                    i:= i+0.001;
//                  end;
//    glEnd();
//    glBegin(GL_LINE_LOOP);
//            i:=0;
//            while (i<2*pi) do
//                  begin
//                    glVertex3f( 0,LoopRadius *  sin(i),LoopRadius *  cos(i));
//                    i:= i+0.1;
//                  end;
//    glEnd();
//
//    //Draw text axis labels
//    glPushMatrix();
//      glLineWidth(1.5);
//      glRotatef(180, 1, 0, 0);
//      glColor4f(1,0,0,0.5); //Red
//      glTextOut( 1.2,0,0, 0.15, 0.2, 0.2, 1, 'X');
//      glColor4f(0,0,1,0.5); //Blue
//      glTextOut( 0,0,-1.2, 0.15, 0.2, 0.2, 1, 'Y');
//      glColor4f(0,1,0,0.5); //Green
//      glTextOut( 0,-1.2,0, 0.15, 0.2, 0.2, 1, 'Z');
//    glPopMatrix();
//
//    //Grid axis lines
//    glBegin(GL_LINES);
//      glColor4f(1,0,0,0.3);//Red
//      glVertex3f(0,0,0);
//      glVertex3f(1.1,0,0);
//      glColor4f(0,1,0,0.3);//Green
//      glVertex3f(0,0,0);
//      glVertex3f(0,1.1,0);
//      glColor4f(0,0,1,0.3);//Blue
//      glVertex3f(0,0,0);
//      glVertex3f(0,0,1.1);
//    glEnd();
//
//    //Value 1
//    glLineWidth(3);
//    glBegin(GL_LINES);
//      glColor4f(1,0.5,0.5,0.9); //Red
//      glVertex3f(0,0,0);
//      glVertex3f(X1, 0,0); // Left/right  value
//      glColor4f(0.5, 1, 0.5,0.9); //Green
//      glVertex3f(0,0,0);
//      glVertex3f( 0,Z1,0); // Up/down Axis
//      glColor4f(0.5,0.5,1,0.9); //Blue
//      glVertex3f(0,0,0);
//      glVertex3f(0,0, Y1); // In/out axis
//      glColor4f(1, 1, 1,0.9); //White
//      glVertex3f(0,0,0);
//      glVertex3f(X1, Z1, Y1); //Combined vector
//    glEnd();
//    //Cube around value 1
//    glLineWidth(1);
//    glColor4f(1,1,1,0.5); //white
//    glBegin(GL_LINES);
//      glVertex3f(0,Z1,0);//X axis lines
//      glVertex3f(X1,Z1,0);
//      glVertex3f(0,Z1,Y1);
//      glVertex3f(X1,Z1,Y1);
//      glVertex3f(0,0,Y1);
//      glVertex3f(X1,0,Y1);
//
//      glVertex3f(X1,0,0);//Y axis lines
//      glVertex3f(X1,0,Y1);
//      glVertex3f(X1,Z1,0);
//      glVertex3f(X1,Z1, Y1);
//      glVertex3f(0,Z1,0);
//      glVertex3f(0,Z1,Y1);
//
//      glVertex3f(X1,0,0);//X axis lines
//      glVertex3f(X1,Z1,0);
//      glVertex3f(0,0,Y1);
//      glVertex3f(0,Z1, Y1);
//      glVertex3f(X1,0,Y1);
//      glVertex3f(X1,Z1,Y1);
//      glEnd();
//
//
//    glPointSize(5);
//    glBegin(GL_POINTS);
//      glColor4f(1,0.5,0.5,0.9); //Red
//      glVertex3f(X1, 0,0); // Left/right  value
//      glColor4f(0.5, 1, 0.5,0.9); //Green
//      glVertex3f( 0,Z1,0); // Up/down Axis
//      glColor4f(0.5,0.5,1,0.9); //Blue
//      glVertex3f(0,0, Y1); // In/out axis
//      glColor4f(1, 1, 1,0.9); //White
//      glVertex3f(X1, Z1, Y1);
//    glEnd();
//
//    //Value 2
//    glLineWidth(2);
//    glBegin(GL_LINES);
//      glColor4f(1,0.5,0.5,0.9); //Red
//      glVertex3f(X1, Z1, Y1);
//      glVertex3f(X1+X2, Z1,Y1); // Left/right  value
//      glColor4f(0.5, 1, 0.5,0.9); //Green
//      glVertex3f(X1, Z1, Y1);
//      glVertex3f(X1, Z1+Z2,Y1); // Up/down Axis
//      glColor4f(0.5,0.5,1,0.9); //Blue
//      glVertex3f(X1, Z1, Y1);
//      glVertex3f(X1, Z1, Y1 + Y2); // In/out axis
//      glColor4f(1, 1, 1,0.9); //White
//      glVertex3f(X1, Z1, Y1);
//      glVertex3f(X1 + X2, Z1 + Z2, Y1 + Y2); //Combined vector
//      glColor4f(1, 1, 0,0.9); //Yellow
//      glVertex3f(X1+X2, Z1+Z2, Y1+Y2); //Line down to ...
//      glVertex3f(X1+X2, 0, Y1+Y2); // ... 0 Z axis
//      glVertex3f(X1+X2, 0, Y1+Y2); //Line back to ...
//      glVertex3f(0, 0, 0); //... center
//    glEnd();
//
//    //Cube around value 2
//    glLineWidth(1);
//    glColor4f(1,1,1,0.5); //white
//    glBegin(GL_LINES);
//      glVertex3f(X1   ,Z1+Z2,Y1);//X axis lines
//      glVertex3f(X1+X2,Z1+Z2,Y1);
//      glVertex3f(X1   ,Z1+Z2,Y1+Y2);
//      glVertex3f(X1+X2,Z1+Z2,Y1+Y2);
//      glVertex3f(X1   ,Z1   ,Y1+Y2);
//      glVertex3f(X1+X2,Z1   ,Y1+Y2);
//
//      glVertex3f(X1+X2,Z1   ,Y1);//Y axis lines
//      glVertex3f(X1+X2,Z1   ,Y1+Y2);
//      glVertex3f(X1+X2,Z1+Z2,Y1);
//      glVertex3f(X1+X2,Z1+Z2,Y1+Y2);
//      glVertex3f(X1   ,Z1+Z2,Y1);
//      glVertex3f(X1   ,Z1+Z2,Y1+Y2);
//
//      glVertex3f(X1+X2,Z1   ,Y1);//X axis lines
//      glVertex3f(X1+X2,Z1+Z2,Y1);
//      glVertex3f(X1   ,Z1   ,Y1+Y2);
//      glVertex3f(X1   ,Z1+Z2,Y1+Y2);
//      glVertex3f(X1+X2,Z1   ,Y1+Y2);
//      glVertex3f(X1+X2,Z1+Z2,Y1+Y2);
//      glEnd();
//
//    glPointSize(4);
//    glBegin(GL_POINTS);
//      glColor4f(1,0.5,0.5,0.9); //Red
//      glVertex3f(X1+X2, Z1, Y1); // Left/right  value
//      glColor4f(0.5, 1, 0.5,0.9); //Green
//      glVertex3f(X1, Z1+Z2, Y1); // Up/down Axis
//      glColor4f(0.5,0.5,1,0.9); //Blue
//      glVertex3f(X1, Z1, Y1+Y2); // In/out axis
//      glColor4f(1, 1, 1,0.9); //White
//      glVertex3f(X1+X2, Z1+Z2, Y1+Y2); //Combined vector
//    glEnd();
//
//  glPopMatrix();
//end;

procedure GetAccel();
var
  pieces: TStringList;
  i : integer;
  result: String;
begin

  pieces := TStringList.Create;
  pieces.Delimiter := ',';

  try
    //Do not show or log value reads while monitoring
    result:=SendGet('v2x',False, 3000, True, MonitorHidesSendGet);
    pieces.DelimitedText := result;

    //if SmoothedCheckBox.Checked then
    if False then begin
           //shift
           for i:= smoothbuffersize-1 downto 1 do begin
                Axb[i]:=Axb[i-1];
                Ayb[i]:=Ayb[i-1];
                Azb[i]:=Azb[i-1];
              end;

           //insert latest reading and invert Y and Z according to fig8/table2 AN3192.pdf
           Axb[0]:= -1.0 * StrToFloatDef(pieces.Strings[1],0);
           Ayb[0]:=        StrToFloatDef(pieces.Strings[2],0);
           Azb[0]:=        StrToFloatDef(pieces.Strings[3],0);

           //clear sum
           Ax:=0;
           Ay:=0;
           Az:=0;

           //sum
           for i:=0 to smoothbuffersize-1 do begin
                Ax:=Ax+Axb[i];
                Ay:=Ay+Ayb[i];
                Az:=Az+Azb[i];
              end;
           //average
           Ax:=Ax/smoothbuffersize;
           Ay:=Ay/smoothbuffersize;
           Az:=Az/smoothbuffersize;
      end
    else begin
      Ax:= -1.0 * StrToFloatDef(pieces.Strings[1],0);
      Ay:=        StrToFloatDef(pieces.Strings[2],0);
      Az:=        StrToFloatDef(pieces.Strings[3],0);
    end;

    if Az=24539 then StatusMessage('Accel=24539, cold-boot meter.');

    //Writeln('GetAccel OK');//debug

    NormalizeAccel();
  except begin
    if VectorForm.InitialErrorLabel.Caption='' then
    VectorForm.InitialErrorLabel.Caption:='GetAccel exception: '+result;
    StatusMessage('GetAccel exception: '+result);
    end;
  end;
  if Assigned(pieces) then FreeAndNil(pieces);
end;

//Normalizes A_ to A_1
//Put raw values A_ into matrix W
procedure NormalizeAccel();
var
  row,col:Integer;
  sum: Double;
begin

  try
    if not GotAccCal then GetAccelCal();

    //get raw accelerometer values in preparation for normalizing
    Wraw.setv(0, 0, Ax);
    Wraw.setv(1, 0, Ay);
    Wraw.setv(2, 0, Az);

    //Set M (multiplier) values from X
    for row:=0 to 2 do
       for col:=0 to 2 do
          M.setv(col,row, X.getv(row,col)); // Also transpose the matrix while reading

    //Set B (offset) values from X
    for row:=0 to 2 do
       B.setv(row,0, X.getv(3,row));

    //Normalize readings
    Ynorm:=M* Wraw + B;

    //Limit within +/- 1.0
    Ynorm.setv(0, 0, EnsureRange(Ynorm.getv(0, 0),-1.0,1.0));
    Ynorm.setv(1, 0, EnsureRange(Ynorm.getv(1, 0),-1.0,1.0));
    Ynorm.setv(2, 0, EnsureRange(Ynorm.getv(2, 0),-1.0,1.0));

    //Pull out normalized values if within deadband
    sum:=sqrt(Ynorm.getv(0, 0)**2 + Ynorm.getv(1, 0)**2 + Ynorm.getv(2, 0)**2);
    //if ((sum<(1.0+DeadbandSpinEdit.Value/100.0)) and (sum>(1.0-DeadbandSpinEdit.Value/100.0)))then
    if ((sum<2.0) and (sum>0.0))then
    begin
      Ax1:=Ynorm.getv(0, 0);
      Ay1:=Ynorm.getv(1, 0);
      Az1:=Ynorm.getv(2, 0);
    end;

    //Writeln('NormalizeAccel OK');//debug

  except
    StatusMessage('NormalizeAccel exception.');
  end;

end;

//normalize the value in Mx, My, Mz and save to Mx1, My1, Mz1
procedure NormalizeMag();
begin

  //Compute offsets
  MxOff:=(MxMin+Mxmax)/2;
  MyOff:=(MyMin+Mymax)/2;
  MzOff:=(MzMin+Mzmax)/2;



  //if MagnetoCheckBox.Checked then
  if False then begin
       try
         //Alternate calibrated calculation based on Magneto 1.2 values
         Mh.setv(0,0,Mx);
         Mh.setv(1,0,My);
         Mh.setv(2,0,Mz);
         MhCal:=MAinv * (Mh - Mb);
         Mx1:=MhCal.getv(0,0)/3200;
         My1:=MhCal.getv(1,0)/3200;
         Mz1:=MhCal.getv(2,0)/3200;
       except
         StatusMessage('Magneto settings exception');
       end;
  end
  else begin
    //Apply offset and Normalize readings
    Mx1:=EnsureRange((Mx-MxOff)/((Mxmax-MxMin)/2),-1.0,1.0);
    My1:=EnsureRange((My-MyOff)/((Mymax-MyMin)/2),-1.0,1.0);
    Mz1:=EnsureRange((Mz-MzOff)/((Mzmax-MzMin)/2),-1.0,1.0);
  end;

  //Put normalized magnetic readings into matrix
  Mnorm.setv(0, 0, Mx1);
  Mnorm.setv(1, 0, My1);
  Mnorm.setv(2, 0, Mz1);
end;


//Get magnetometer readings from meter into Mx, My, Mz
// Optionally hide results from logfile (useful for continuous monotoring of compass).
// Offsets are applied to produce Mx1, My1, Mz1
// Matrix Mnorm is filled.
procedure GetMag(Hide:Boolean=False);
var
   pieces: TStringList;
   i : integer;
   result: String;
const
  Mmax=5000;
  Mmin=-5000;
begin

  try
    if not GotMagCal then GetMagCal();

    pieces := TStringList.Create;
    pieces.Delimiter := ',';

    //Get magnetometer values, and optionally "Hide" result.
    result:=SendGet('v1x',False, 3000, True, Hide);

    //Parse magnetometer values.
    pieces.DelimitedText := result;

    if pieces.Count<3 then
       StatusMessage(format('Getmag pieces count < 4 %s',[result]));

    //writeln('getmag pieces.Count=',pieces.Count);//debug

    //if SmoothedCheckBox.Checked then
    if False then begin
         //shift
         for i:= smoothbuffersize-1 downto 1 do begin
              Mxb[i]:=Mxb[i-1];
              Myb[i]:=Myb[i-1];
              Mzb[i]:=Mzb[i-1];
         end;

         //insert latest reading and invert Y and Z according to fig8 AN3192.pdf
         Mxb[0]:=        StrToFloatDef(pieces.Strings[1],0);
         Myb[0]:= -1.0 * StrToFloatDef(pieces.Strings[2],0);
         Mzb[0]:= -1.0 * StrToFloatDef(pieces.Strings[3],0);

         //clear sum
         Mx:=0;
         My:=0;
         Mz:=0;

         //sum
         for i:=0 to smoothbuffersize-1 do begin
              Mx:=Mx+Mxb[i];
              My:=My+Myb[i];
              MZ:=Mz+Mzb[i];
         end;
         //average
         Mx:=Mx/smoothbuffersize;
         My:=My/smoothbuffersize;
         Mz:=Mz/smoothbuffersize;
    end
    else begin
      Mx:=        StrToFloatDef(pieces.Strings[1],0);
      My:= -1.0 * StrToFloatDef(pieces.Strings[2],0);
      Mz:= -1.0 * StrToFloatDef(pieces.Strings[3],0);
    end;

    //debug super large values.
    //if abs(Mx)>Mmax then writeln(Mx, ' ' ,result);
    //if abs(My)>Mmax then writeln(My, ' ' ,result);
    //if abs(Mz)>Mmax then writeln(Mz, ' ' ,result);


    //Compute new min/max magnetic values
    if MinMaxMagCheck then begin
      if ((Mx<Mxmin) and (Mx>Mmin)) then Mxmin:= Mx;
      if ((My<Mymin) and (My>Mmin)) then Mymin:= My;
      if ((Mz<Mzmin) and (Mz>Mmin)) then Mzmin:= Mz;

      if ((Mx>Mxmax) and (Mx<Mmax)) then Mxmax:= Mx;
      if ((My>Mymax) and (My<Mmax)) then Mymax:= My;
      if ((Mz>Mzmax) and (Mz<Mmax)) then Mzmax:= Mz;
    end;

    NormalizeMag();

    //writeln('getmag was OK');//debug

  except
    begin
      if VectorForm.InitialErrorLabel.Caption='' then
      VectorForm.InitialErrorLabel.Caption:='GetMag exception: '+result;
      StatusMessage('GetMag exception: '+result);
    end;
  end;
  if Assigned(pieces) then FreeAndNil(pieces);
end;

initialization
  {$I vector.lrs}

end.

