function [A,C,K] = armodel(samples, nComponentsRemoved)
% [A,C,K] = armodel(samples, nComponentsRemoved)
% Function returns A, C, and K matricies for an AR model
% input: samples - each column is a vector in a time sequence
%        nComponentsRemoved - number of principle components removed
%
%                 Model
%                     x(k+1)  =   A*x(k)  +   w(k)
%                     y(k)    =   C*x(k)  +   v(k)
%
% code written by Ashok Veeraraghavan
% modified by James Sherman



featurelength = size(samples, 1);
% number of principle components, arbitrary
No_pca = featurelength-nComponentsRemoved; 


        %Original PCA Routine.        
        
        %[pn,meanp,stdp] = prestd(samples);        
        %[ptrans,transMat] = prepca(pn,0.00001);
        %reconstr    =   ptrans(1:No_pca,:);
        
        %Original PCA Routine ends here.
        
        
%*************************************************************************************************************
%New PCA routine using SVD
%Just Like Soatto

%   Notation
%       eigu = eigenvectors
%       sigma * eigv' = Weights for the eigen vectors
%       
%                 Model
%                     x(k+1)  =   A*x(k)  +   w(k)
%                     y(k)    =   C*x(k)  +   v(k)
%                     
%                     where
%                     E[w(k)w(k)']    =   Q
%                     E[v(k)v(k)']    =   R
%                     E[w(k)v(k)']    =   S
%                     Forward Innovation Model Kalman Gain =  K
%                     
%                     E[x(k)v(k)']    =   0
%                     E[x(k)w(k)']    =   0
%                     
%                     E[x(k)x(k)']    =   zigma
%                     E[y(k)y(k)']    =   lambda
%                     
%                     By defn:
%                     G   =   A*zigma*C'  +   S;
%                     
        
        
        
        [eigu sigma eigv] =   svd(samples);
        sigmanew    =   zeros(size(sigma));
        for i=1:No_pca
            sigmanew(i,i)   =    sigma(i,i);
        end
        singularvalues      =   sigmanew(1:No_pca,1:No_pca);
        smoothedsamples     =   eigu*sigmanew*eigv';
        weights             =   sigmanew*eigv';
        reconstr            =   weights(1:No_pca,:);
        
        %#######################################################
        %Lets compute the various correlation matrices that are required.
        %#######################################################

        %Now computing      E[v(k)v(k)']    =   R 
        %----------------------------------------------
        error_pca   =   samples -   smoothedsamples;
        [m_error_pca,n_error_pca]   =   size(error_pca);
        R=zeros(m_error_pca,m_error_pca);
        
        for i=1:n_error_pca
            R   =   R   +   error_pca(:,i)*error_pca(:,i)';
        end
        R   =   R/n_error_pca;
        %disp('R')
        %----------------------------------------------------
        
        %Now Computing C (the eigenvectors)
        %----------------------------------------------------
        C   =   eigu(:,1:No_pca);
        %disp('C');
        %-----------------------------------------------------
        
        
        %Now computing          E[x(k)x(k)']    =   zigma
        %-----------------------------------------------------
        temp_x  =   weights(1:No_pca,:);
        temp_x  =   temp_x  -   mean(temp_x,2)*ones(1,size(temp_x,2));
        [m_temp_x,n_temp_x] =   size(temp_x);
        zigma   =   zeros(m_temp_x,m_temp_x);
        
        for i=1:n_temp_x
            zigma   =   zigma   +   temp_x(:,i)*temp_x(:,i)';
        end
        zigma   =   zigma/n_temp_x;
          
        %disp('zigma');
        %-------------------------------------------------------
        
        
        %Now Computing the Transition Matrix A
        %--------------------------------------------------------
        temp_eigv  =   eigv(:,1:No_pca);
        No_frame  =   size(samples,2);
        D1      =   [zeros(1,No_frame);eye(No_frame-1) zeros(No_frame-1,1)];
        D2      =   [eye(No_frame-1) zeros(No_frame-1,1);zeros(1,No_frame)];
        
        
        A   =   singularvalues*temp_eigv'*D1*temp_eigv*(inv(temp_eigv'*D2*temp_eigv))*inv(singularvalues);
        %disp('A');
        
        %--------------------------------------------------------------
        
        
        %Now computing      E[y(k)y(k)']    =   lambda
        %----------------------------------------------------------------
        [m_samples,n_samples]   =   size(samples);
        lambda  =   zeros(m_samples,m_samples);
        
        for i=1:n_samples
            lambda  =   lambda  +   samples(:,i)*samples(:,i)';
        end
        lambda  =   lambda/n_samples;
        %disp('lambda');
        %------------------------------------------------------------------
        
        
        %Now Computing          E[w(k)v(k)']    =   S
        %--------------------------------------------------------------------
        temp_w      =   samples -   smoothedsamples;
        temp_predx  =   temp_x;
        [m_temp_x,n_temp_x] =   size(temp_x);
        
        for i=2:n_temp_x
            temp_predx(:,i) =   A*temp_x(:,i-1);
        end
        temp_v  =   temp_x  -   temp_predx;
        
        S   =   zeros(m_samples,m_temp_x);
        for i=1:n_temp_x
            S   =   S   +   temp_w(:,i)*temp_v(:,i)';
        end
        
        S   =   S/n_temp_x;
        %disp('S');
        %**********************
        %S=zeros(size(S));
        %**********************
        
        %---------------------------------------------------------------------
        
        
        %Now Computing              G   =   A*zigma*C'  +   S;
        %---------------------------------------------------------------------
        G   =   A*zigma*C'  +   S';
        %disp('G');
        %----------------------------------------------------------------------
        
        
        
        %&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
        
        % Have to find a method of computing the Kalman Gain
        
        
        %Now Computing P from the Riccati Eqn 
        %               P =   APA'    +   (G-APC')*inv(lambda-CPC')*(G-APC')';
        %-----------------------------------------------------------------------
        %[P,nap1,nap2,nap3]  =   dare(A',C',zeros(size(A')),-lambda,-G);
        %%disp('P');
        %-----------------------------------------------------------------------
        
        %Now Computing the Kalman Gain K
        %-----------------------------------------------------------------------
        %K   =   (G-A*P*C')*inv(lambda-C*P*C');
        %disp('K');
        %-----------------------------------------------------------------------
        
        %Computing K as given in Page 10 of Subspace Algorithms for the stochastic identification Problem.
        K   =   (G-A*zigma*C')*pinv(lambda-C*zigma*C');
        %disp('K');

           
% $$$         cd SystemMatrices
% $$$         cd(subdir)
% $$$          
% $$$         matfile =   sprintf('System%02d',shortid(s));
% $$$         save(matfile,'A','C','K');
% $$$         cd ..
% $$$         cd ..
% $$$      
        
        
        %Just Like Soatto
        %New PCA routine ends here
        
        
        
        
        
        
        
        
        %If u want to bypass PCA use this
        %reconstr     =  samples;
        %ends Pca routines       
        %Change Ends here
        
        %********************************************************************************************************************
       
%    end
%end