%
% PURPOSE:  Reformat Rocks .csv data into variables that look like Dorado, so I
%           can use the Dorado plot routines.
% DATE:     26 April 2022
%
 % // for appStatus messages
 % std::stringstream ss;
 % ss << std::setprecision(14)
 %    << pcf::get_timestamp() << ", "              // LRdata(i,1)

 %    << kftMsg.northing_m << ", "                 // LRdata(i,2)
 %    << kftMsg.easting_m << ", "                  // LRdata(i,3)
 %    << kftMsg.depth_m << ", "                    // LRdata(i,4)

 %    << kftMsg.heading_rad << ", "                // LRdata(i,5)
 %    << kftMsg.pitch_rad << ", "                  // LRdata(i,6)
 %    << kftMsg.roll_rad << ", "                   // LRdata(i,7)

 %    << 0 << ", "                                 // LRdata(i,8)
 %    << 0 << ", "                                 // LRdata(i,9)
 %    << 0 << ", "                                 // LRdata(i,10)

 %    << vel_vf(0) << ", "                         // LRdata(i,11)
 %    << vel_vf(1) << ", "                         // LRdata(i,12)
 %    << vel_vf(2) << ", "                         // LRdata(i,13)

 %    << 1 << ", "                                 // LRdata(i,14)
 %    << 1 << ", "                                 // LRdata(i,15)
 %    << 4 << ", "                                 // LRdata(i,16)

 %    << 0 << ", "                                 // LRdata(i,17)
 %    << 1 << ", "                                 // LRdata(i,18)
 %    << (beam1_cm / 100.0) << ", "                // LRdata(i,19)

 %    << 1 << ", "                                 // LRdata(i,20)
 %    << 1 << ", "                                 // LRdata(i,21)
 %    << (beam2_cm / 100.0) << ", "                // LRdata(i,22)

 %    << 2 << ", "                                 // LRdata(i,23)
 %    << 1 << ", "                                 // LRdata(i,24)
 %    << (beam3_cm / 100.0) << ", "                // LRdata(i,25)

 %    << 3 << ", "                                 // LRdata(i,26)
 %    << 1 << ", "                                 // LRdata(i,27)
 %    << (beam4_cm / 100.0)                        // LRdata(i,28)
 %    << std::endl;

 if(~exist('fileName'))
   fprintf('Please set fileName = ''something'' before execting this.\n');
 end
 
 rockcsv = csvread(fileName,4,0);
 
 dataNav = rockcsv(:,[1:7]);
 mPos_x = 2;
 mPos_y = 3;
 mDepth = 4;
 mPhi   = 7;
 mTheta = 6;
 mPsi   = 5;
 vel_N = rockcsv(:,[11 12 13]);
 psi = rockcsv(:,mPsi);
% T_BN = [ cos(psi) sin(psi)
%         -sin(psi) cos(psi)];
