// P78MixHiMix.sce
// Estimation of hierarchical mixture
// - upper - SPEED (with three normal components)
// - lower - SPEED DIFFERENCE and CROSS ACCELERATION
//           (economical and sport-like driving is to be detected)
//   absolute values of data -> first quadrant
//   two lower components: 1. HNm, 2. Exp 
// ----------------------------------------------------------
[u,t,n]=file();                         // find working directory
chdir(dirname(n(2)));                   // set working directory
clear("u","t","n")                      // clear auxiliary data
exec("ScIntro.sce",-1),mode(0)          // intro to sesion

nd=1500;                      // number of data for estimation
ni=50;                        // virtual length of initialization

load  _data/dataOkruhy.dat;         // real data from a car
dat=dataOkruhy.dataOrig(:,3001:10:$);
dat=dat(:,3601:7240);

// data
v=dat(2,1:nd);                // upper data - velocity
d=abs([diff(v) 0]);           // abs. values of velocity variations
a=20*abs(dat(9,1:nd));        // abs. values of cross-acceleration

// variables
yt=v;                         // data -> varibles (speed)
dt=[d;a]+1e-8;                // data -> varibles (accelerations)

// INITIAL PARAMETERS ============================================
thC0=[50 70 100];             // speeds in: town, out-town, motorway
thP0=[.2 .5 .3];              // probabilities of going in -"-
nc0=length(thC0);             // number of upper components

nv=2;                         // number of lower variables
nc=[2 2 2];                   // number of lower components

w0=fnorm(ones(1,nc0));        // initial upper weights
w=list();
for i=1:nc0                   // initial lower weights
  w(i)=3+rand(1,nc(i));
  w(i)=w(i)/sum(w(i));
end

thCr=list(); thCe=list();
// parameters of lower components
// - the data are: - d1 long. acceleration (mean value is 2.6
//                 - d2 cross accel. (mean after normaliz. is 3.8
//           d1 d2
thCe(1)=      [1 .2]';        // Exp (1st comp. of 1st mixture)
thCr(1)=%pi/2*[8 4]';        // HNm (2nd komp. of 1st mixture)

thCe(2)=      [1 .2]';         // 2nd mixture
thCr(2)=%pi/2*[8 4]';        //   expectation -> statistics

thCe(3)=      [1 .2]';         // 3rd mixture
thCr(3)=%pi/2*[8 4]';

// INITIALIZATION ================================================
// UPPER mixture
for j=1:nc0 
  Ps=[thC0(:,j);1];           // Regression vector
  Cyp.V=Ps*Ps';
  Est0.Cy(j)=Cyp;             // information matrix
  Est0.Cy(j).th=thC0(:,j);    // parameters
  Est0.Cy(j).sd=.1;           // std of noise
end
Est0.ka=1*rand(1,nc0,'u');    // counter
Est0.Cp.V=rand(nc0,nc0,'u')+1;// statistics for pointer estimation
Est0.Cp.th=fnorm(Est0.Cp.V,2);// parameters of pointer model
Est0.ct=1;                    // initial value of the pointer
Est0.nc=nc0;                  // number of upper components
Est0.w=w0;                    // initial weight
Est0.W=zeros(nc0,nc0);
Est0.tht=zeros(nc0,nd);
Est0.wt=zeros(nc0,nd);

// LOWER mixture
for i=1:nc0
  Esti.i=i;                   // label of lower component
  Esti.ka=ni*ones(1,2);       // counter
  // first component
  Esti.Cy(1).V=ni*thCe(i);    // statistics
  Esti.Cy(1).th=thCe(i);      // initial parameters
  // second component
  Esti.Cy(2).V=ni*thCr(i);       // statistics
  Esti.Cy(2).th=.8*thCr(i);   // initial parameters

  Esti.Cp.V=rand(1,nc(i),'u')+1; // pointer statistics
  Esti.Cp.th=fnorm(Esti.Cp.V);   // pointer parameters

  Esti.nc=nc(i);              // number of lower components
  Esti.w=w(i);                // initial weights
  Esti.ct=zeros(1,nd); 
  
  Est(i)=Esti;                // lower component construction
end
//pause
// ESTIMATION loop ===============================================
tic();
kk=ceil(nd/10); printf('\n  2 4 6 8 |\n ')
for t=1:nd  
if t/kk==fix(t/kk), printf('.'); end

  [Est0,Est]=MEst78(yt(:,t),dt(:,t),Est0,Est);
    [xxx,Est0.ct(1,t)]=max(Est0.w);
    Est0.wt(:,t)=Est0.w';
    Est0.tht(:,t)=[Est0.Cy(1).th,Est0.Cy(2).th,Est0.Cy(3).th]';
  
  for i=1:nc0
    Est(i).tht(:,t)=[Est(i).Cy(1).th(:);Est(i).Cy(2).th(:)];
    [xxx,Est(i).ct(t)]=max(Est(i).w);
    Est(i).wt(:,t)=Est(i).w';
  end
end
// end of estimation loop ========================================
disp ' ', time=toc()/60; printf(' time = %5.2f min.\n',time)

// RESULTS
d=list();
for i=1:3
  m=find(Est0.ct==i);
  d(i)=dt(:,m); 
  for j=1:2
    k=find(Est(i).ct(m)==j);
    Ct.t=d(i)(:,k);
    E(i).C(j)=Ct;             // data in components
  end
end

// data clusters colored with respect to components
tx=['in town';'outside town';'motorway'];
set(scf(112),'position',[400 150 1000 350]);
for i=1:3
  subplot(1,3,i)
  plot(E(i).C(1).t(1,:),E(i).C(1).t(2,:),'b.','markersize',3)
  plot(E(i).C(2).t(1,:),E(i).C(2).t(2,:),'r.','markersize',3)
  title(tx(i))
  set(gca(),'data_bounds',[0 30 0 20]);
  legend('economic driving','sport driving');
end

