// manipulate and display the attitude
// V is [ degrees, volts ]
V=[
 -360, 2.967
 -270, 2.843
 -180, 2.716
  -90, 2.598
    0, 2.477
   90, 2.352
  180, 2.235
  270, 2.105
  360, 1.990
];

n=size(V,1);
vcc = 4.96;  // volts

// The equation is V(:,2)*x+b = V(:,1) and x is [intercept; slope]
// form this as V(i,2)+b, so, add a column of ones to V(:,2)
X=[ones(n,1),V(:,2)];  // a n row, 2 column matrix
b=inv(X'*X)*X'*V(:,1);   // least squares, assuming X'*X has an inverse
// beta now contains [intercept; slope]
slope=b(2); intercept=b (1);
//printf("Least Squares, angle = %.2f*volts + %.2f\n", slope, intercept);


///////////////////////////////////////////////////////////
// read the data
meas=fscanfMat('attitude.dat');
n=size(meas,1);
// trim the last entry, it may be bogus
meas=meas(1:n-1,:);
n=size(meas,1);

/////////////////////////////////////////////////////////////
// sample time, etc
sampleTime=mean(meas(2:n,3)-meas(1:n-1,3))/1000;  // Nominal 30 msec, but

// angle generated by the pot
pot=meas(:,8);
p=size(pot,1);
// have to smooth the data, some
pot_max_change = 50;
for (i=1:p-1)
  if (abs(pot(i+1)-pot(i))>pot_max_change)
    pot(i+1) = pot(i);
  end
end
// calculate the angle produced by the pot
pot_angle = slope*(vcc*pot/1024) + intercept;

hdg_hat = mean(meas(:,4));
tilt_hat = mean(meas(:,6));

///////////////////////////////////////////////////////////
// remember, the data is left shifted one bit
n = size(meas,1);
k=20/3;             // mv/deg/sec

// heading
V=(25/12)*meas(:,4)/2+400;  // V is mv
M=mean(V(1:100));         // mean voltage, use the first 100 for calibration
Zmeas=cumsum(X=(V-M)/k)*sampleTime;

// tilt - same as heading, except different meas column
V=(25/12)*meas(:,6)/2+400;  // V is mv
Xmeas=cumsum(X=(V-M)/k)*sampleTime;

///////////////////////////////////////////////////////////
// convert the accel data to something like pot_angle
function [soln]=filter(cutoff, order)
  sampleRate=1/sampleTime ;
  lisys1=iir(order,'lp','butt',[cutoff/sampleRate 0],[0,0]);
  resul1=flts(yxAng',lisys1);
  n=max(size(resul1));
  soln=flts(resul1(n:-1:1),lisys1);
  resul2=flts(yAccel',lisys1);
  resul3=flts(zAccel',lisys1);
  soln=resul1;
endfunction;

function [X]=accel_adj(X)
  m=mean(X);
  X(X>2000)  = m;
  X(X<-2000) = m;
endfunction;

xAccel=accel_adj(meas(:,9) - mean(meas(10:400,9)));
yAccel=accel_adj(meas(:,10)- mean(meas(10:400,10)));
zAccel=accel_adj(meas(:,11)- mean(meas(10:400,11)));

yxAng=atan(yAccel,xAccel);
zyAng=atan(zAccel,yAccel);
xzAng=atan(xAccel,zAccel);

AccelAng=filter(0.2,3);

///////////////////////////////////////////////////////////
// tick labels
xtmax=ceil(max(meas(:,3))/100000)*100;
xtdelta=xtmax/10;
xtlabel=string([0:xtdelta:xtmax]');

//set("figure_style","new"); //create a figure
xbasc();
drawlater();

subplot(4,2,1);
plot(meas(:,3),meas(:,4));
a=get("current_axes");
a.x_ticks.labels=xtlabel;
a.x_label.text="";
a.y_label.text="Counts";
a.title.text="Heading Gyro - Raw Data";
//a.title.font_size=2;

subplot(4,2,3);
plot(meas(:,3),meas(:,6));
a=get("current_axes");
a.x_ticks.labels=xtlabel;
a.y_label.text="Angle";
a.title.text="Tilt Gyro - Raw Data";
//a.title.font_size=2;

subplot(4,2,5);
plot(meas(:,3),Zmeas);
a=get("current_axes");
a.x_ticks.labels=xtlabel;
a.y_label.text="Angle";
xtitle("Heading Gyro - Calculated","","Angle");
//a.title.font_size=2;

subplot(4,2,7);
plot(meas(:,3),pot_angle);
a=get("current_axes");
a.x_ticks.labels=xtlabel;
a.y_label.text="Angle";
a.title.text="Potentiometer Angle";
a.x_label.text="Seconds";
//a.title.font_size=2;

subplot(4,2,2);
plot(meas(:,3),xAccel);
a=get("current_axes");
a.x_ticks.labels=xtlabel;
a.title.text="X Accel - Raw Data";
a.y_label.text="mG";
//a.title.font_size=2;

subplot(4,2,4);
plot(meas(:,3),yAccel);
a=get("current_axes");
a.x_ticks.labels=xtlabel;
a.y_label.text="Counts";
a.title.text="Y Accel - Raw Data";
//a.title.font_size=2;

subplot(4,2,6);
plot(meas(:,3),zAccel);
a=get("current_axes");
a.x_ticks.labels=xtlabel;
a.y_label.text="Counts";
a.title.text="Z Accel - Raw Data";
//a.title.font_size=2;

subplot(4,2,8);
plot(meas(:,3),AccelAng,'b');
plot(meas(:,3),pot_angle/max(pot_angle),'r');
a=get("current_axes");
a.x_ticks.labels=xtlabel;
a.y_label.text="mG";
a.title.text="X Accel - IIR Filtered Data (red pot)";
a.x_label.text="Seconds";
//a.title.font_size=2;

drawnow();