clc;
survey_fn = 'Survey_Calgary_COLDEX_2023_08_04.xlsx';

%% LEFTFORWARD spreadsheet
fprintf('READ LEFTFORWARD ==========================================================================\n');
read_table_opts = detectImportOptions(survey_fn,'NumHeaderLines',0,'Sheet','LEFTFORWARD','ReadVariableNames',false);
read_table_opts.DataRange = 'A1';
for var_idx = 1:length(read_table_opts.VariableTypes)
  read_table_opts.VariableTypes{var_idx} = 'char';
end
read_table_output = readtable(survey_fn,read_table_opts);
txt = table2cell(read_table_output);
num = nan(size(txt));
for element_idx = 1:numel(txt)
  num(element_idx) = str2double(txt{element_idx});
  if ~isnan(num(element_idx))
    txt{element_idx} = '';
  end
end
clear('read_table_opts','read_table_output','var_idx','element_idx');

s = struct();

row = 4;
s.GPS_CG.E = num(row,3);
s.GPS_CG.N = num(row,2);
s.GPS_CG.Z = num(row,4);

row = 5;
s.GPS_Tail.E = num(row,3);
s.GPS_Tail.N = num(row,2);
s.GPS_Tail.Z = num(row,4);

row = 6;
s.For_Centerline.E = num(row,3);
s.For_Centerline.N = num(row,2);
s.For_Centerline.Z = num(row,4);

row = 7;
s.Aft_Centerline.E = num(row,3);
s.Aft_Centerline.N = num(row,2);
s.Aft_Centerline.Z = num(row,4);

num_rows = size(txt,1);
for row = 8:5:num_rows
  field_name = txt{row,1};
  field_name(field_name==' ') = '_';
  for row_offset = 1:4
    sub_field_name = txt{row+row_offset,1};
    sub_field_name(sub_field_name==' ') = '_';
    s.([field_name '_' sub_field_name]).E = num(row+row_offset,3);
    s.([field_name '_' sub_field_name]).N = num(row+row_offset,2);
    s.([field_name '_' sub_field_name]).Z = num(row+row_offset,4);
  end
end

fprintf('Shift s ==========================================================================\n')

origin = s.For_Centerline;
s.For_Centerline
s.Aft_Centerline

s_fieldnames = fieldnames(s);
for field_idx = 1:length(s_fieldnames)
  s.(s_fieldnames{field_idx}).E = s.(s_fieldnames{field_idx}).E - origin.E;
  s.(s_fieldnames{field_idx}).N = s.(s_fieldnames{field_idx}).N - origin.N;
  s.(s_fieldnames{field_idx}).Z = s.(s_fieldnames{field_idx}).Z - origin.Z;
end

s.For_Centerline
s.Aft_Centerline

fprintf('Rotate s ==========================================================================\n')

% Z-rotation using ENU, east-north-up coordinate system
x_axis = [s.For_Centerline.E-s.Aft_Centerline.E; s.For_Centerline.N-s.Aft_Centerline.N; s.For_Centerline.Z-s.Aft_Centerline.Z];
theta = atan2(-x_axis(2),x_axis(1));
rot_matrix = [cos(theta) -sin(theta) 0; sin(theta) cos(theta) 0; 0 0 1];

s_fieldnames = fieldnames(s);
for field_idx = 1:length(s_fieldnames)
  vec = [s.(s_fieldnames{field_idx}).E; s.(s_fieldnames{field_idx}).N; s.(s_fieldnames{field_idx}).Z];
  new_vec = rot_matrix * vec;
  s.(s_fieldnames{field_idx}).E = new_vec(1);
  s.(s_fieldnames{field_idx}).N = new_vec(2);
  s.(s_fieldnames{field_idx}).Z = new_vec(3);
end

s.For_Centerline
s.Aft_Centerline

% Y-rotation
x_axis = [s.For_Centerline.E-s.Aft_Centerline.E; s.For_Centerline.N-s.Aft_Centerline.N; s.For_Centerline.Z-s.Aft_Centerline.Z];
theta = atan2(x_axis(3),x_axis(1));
rot_matrix = [cos(theta) 0 sin(theta); 0 1 0; -sin(theta) 0 cos(theta)];

s_fieldnames = fieldnames(s);
for field_idx = 1:length(s_fieldnames)
  vec = [s.(s_fieldnames{field_idx}).E; s.(s_fieldnames{field_idx}).N; s.(s_fieldnames{field_idx}).Z];
  new_vec = rot_matrix * vec;
  s.(s_fieldnames{field_idx}).E = new_vec(1);
  s.(s_fieldnames{field_idx}).N = new_vec(2);
  s.(s_fieldnames{field_idx}).Z = new_vec(3);
end

s.For_Centerline
s.Aft_Centerline

% Rename and convert from feet to meters
s_fieldnames = fieldnames(s);
for field_idx = 1:length(s_fieldnames)
  s.(s_fieldnames{field_idx}) = [s.(s_fieldnames{field_idx}).E; s.(s_fieldnames{field_idx}).N; s.(s_fieldnames{field_idx}).Z] * 12*2.54/100;
end

% sl = struct left
sl = s;

%% RIGHTFORWARD spreadsheet
fprintf('READ RIGHTFORWARD ==========================================================================\n');
read_table_opts = detectImportOptions(survey_fn,'NumHeaderLines',0,'Sheet','RIGHTFORWARD','ReadVariableNames',false);
read_table_opts.DataRange = 'A1';
for var_idx = 1:length(read_table_opts.VariableTypes)
  read_table_opts.VariableTypes{var_idx} = 'char';
end
read_table_output = readtable(survey_fn,read_table_opts);
txt = table2cell(read_table_output);
num = nan(size(txt));
for element_idx = 1:numel(txt)
  num(element_idx) = str2double(txt{element_idx});
  if ~isnan(num(element_idx))
    txt{element_idx} = '';
  end
end
clear('read_table_opts','read_table_output','var_idx','element_idx');

s = struct();

row = 4;
s.GPS_CG.E = num(row,3);
s.GPS_CG.N = num(row,2);
s.GPS_CG.Z = num(row,4);

row = 5;
s.GPS_Tail.E = num(row,3);
s.GPS_Tail.N = num(row,2);
s.GPS_Tail.Z = num(row,4);

row = 6;
s.For_Centerline.E = num(row,3);
s.For_Centerline.N = num(row,2);
s.For_Centerline.Z = num(row,4);

row = 7;
s.Aft_Centerline.E = num(row,3);
s.Aft_Centerline.N = num(row,2);
s.Aft_Centerline.Z = num(row,4);

num_rows = size(txt,1);
for row = 8:5:num_rows
  field_name = txt{row,1};
  field_name(field_name==' ') = '_';
  for row_offset = 1:4
    sub_field_name = txt{row+row_offset,1};
    sub_field_name(sub_field_name==' ') = '_';
    s.([field_name '_' sub_field_name]).E = num(row+row_offset,3);
    s.([field_name '_' sub_field_name]).N = num(row+row_offset,2);
    s.([field_name '_' sub_field_name]).Z = num(row+row_offset,4);
  end
end

fprintf('Shift s ==========================================================================\n')

origin = s.For_Centerline;
s.For_Centerline
s.Aft_Centerline

s_fieldnames = fieldnames(s);
for field_idx = 1:length(s_fieldnames)
  s.(s_fieldnames{field_idx}).E = s.(s_fieldnames{field_idx}).E - origin.E;
  s.(s_fieldnames{field_idx}).N = s.(s_fieldnames{field_idx}).N - origin.N;
  s.(s_fieldnames{field_idx}).Z = s.(s_fieldnames{field_idx}).Z - origin.Z;
end

s.For_Centerline
s.Aft_Centerline

fprintf('Rotate s ==========================================================================\n')

% Z-rotation using ENU, east-north-up coordinate system
x_axis = [s.For_Centerline.E-s.Aft_Centerline.E; s.For_Centerline.N-s.Aft_Centerline.N; s.For_Centerline.Z-s.Aft_Centerline.Z];
theta = atan2(-x_axis(2),x_axis(1));
rot_matrix = [cos(theta) -sin(theta) 0; sin(theta) cos(theta) 0; 0 0 1];

s_fieldnames = fieldnames(s);
for field_idx = 1:length(s_fieldnames)
  vec = [s.(s_fieldnames{field_idx}).E; s.(s_fieldnames{field_idx}).N; s.(s_fieldnames{field_idx}).Z];
  new_vec = rot_matrix * vec;
  s.(s_fieldnames{field_idx}).E = new_vec(1);
  s.(s_fieldnames{field_idx}).N = new_vec(2);
  s.(s_fieldnames{field_idx}).Z = new_vec(3);
end

s.For_Centerline
s.Aft_Centerline

% Y-rotation
x_axis = [s.For_Centerline.E-s.Aft_Centerline.E; s.For_Centerline.N-s.Aft_Centerline.N; s.For_Centerline.Z-s.Aft_Centerline.Z];
theta = atan2(x_axis(3),x_axis(1));
rot_matrix = [cos(theta) 0 sin(theta); 0 1 0; -sin(theta) 0 cos(theta)];

s_fieldnames = fieldnames(s);
for field_idx = 1:length(s_fieldnames)
  vec = [s.(s_fieldnames{field_idx}).E; s.(s_fieldnames{field_idx}).N; s.(s_fieldnames{field_idx}).Z];
  new_vec = rot_matrix * vec;
  s.(s_fieldnames{field_idx}).E = new_vec(1);
  s.(s_fieldnames{field_idx}).N = new_vec(2);
  s.(s_fieldnames{field_idx}).Z = new_vec(3);
end

s.For_Centerline
s.Aft_Centerline

% Rename and convert from feet to meters
s_fieldnames = fieldnames(s);
for field_idx = 1:length(s_fieldnames)
  s.(s_fieldnames{field_idx}) = [s.(s_fieldnames{field_idx}).E; s.(s_fieldnames{field_idx}).N; s.(s_fieldnames{field_idx}).Z] * 12*2.54/100;
end

% sr = struct rightforward
sr = s;
clear s;



%% UTIG

fprintf('UTIG Position =================================================\n');

% From: Dillon P Buhl <dillon.buhl@utexas.edu>
% Sent: Monday, October 23, 2023 11:37 AM
%
% We do have a good measurement (YYC, pre-CXA1) of the FSAS IMU -> CG GPS antenna leverarm.
%
% In the FSAS's coord system (+x right, +y fwd), from the FSAS to the GPS antenna:
%
% delx = +0.50 m
% dely = +5.43 m
% delz = +2.30 m


% Other background information:
%   On Sat, Oct 21, 2023 at 8:00 AM Gregory Ng <ngg@utexas.edu> wrote:
% Using the mkb weight and balance, I would say that the Riegl LD90 laser
% is listed at station 427, but I'd conservatively apply +- 6 inches to
% that due to interpretation uncertainty.  It is to the right of the
% centerline by 14.5 inches (37 cm), +- 1 inch, measured in ICP5.  The
% aperture of the laser is listed as measured from ICP5 at 18 inches (46
% cm) below the surface floor +- 2 inches, and accounting for the
% relative positioning and size of the laser (23 cm tall) and FSAS (10.4
% cm tall), I would estimate the center of the FSAS to be 18 cm below the
% surface of the floor.  The attached photo shows the FSAS being about 22
% cm (8.8 inches) behind the laser, centered with it.  It looks like the
% bottom of the FSAS is approximately aligned, but slightly below the top
% of the laser.
%
% Summary Estimate for FSAS position CXA1:
%
% Forward/aft: Station 436 +- 6 inches
% Left-right: right of center line 14.5 inches (37 cm) +- 1 inch
% Up-down: 7 inches (18 cm) below floor +- 2 inches


sr.UTIG_IMU = sr.GPS_CG + [-5.43; -0.5; -2.3];
sr.UTIG_IMU

% sr.GPS_CG - sr.Wing_1_Right_Forward
s = sl;
UTIG1 = mean([s.Wing_1_Right_Forward s.Wing_2_Right_Forward],2) - sr.UTIG_IMU
UTIG2 = mean([s.Wing_6_Right_Forward s.Wing_5_Right_Forward],2) - sr.UTIG_IMU






