/*
 *  Copyright 2002 by Soos, Antal
 *  All rights reserved. Property of Soos, Antal.
 *  Restricted rights to use, duplicate or disclose this code are
 *  granted through contract.  
 */
/***************************************************************************/
/*                                                                         */
/*     WCE . cpp	                                                   */
/*                                                                         */
/*     Recoursive Worst Case Estimation Algorithm inplementation.          */
/*                                                                         */
/*                                                                         */
/***************************************************************************/
#define PROB 0
#include "WCE.hpp"

//-----------------------------------------------------------------------------
//Initialization: 
//-----------------------------------------------------------------------------
/*
function [theta,P,delta] = WCE_init(deltai,model)
%  Worst CASE Estimation (WCE) algorithm initiialization
%----------------------------------------------------------
N=model(1,1)+model(1,2)+model(1,3)+model(1,4);
P = 10^12*eye(N,N); % Identity matrix
for i=1:N
theta(i,1) = 0;
end
%Maximum error => delta max(v)
delta = deltai;
*/

WCE_alg::WCE_alg(float delta_WCE) : sysid()						 
{
	int i;
	//eye_matrix_alloc(&P_WCE, ARMAX_dim);	// P_WCE:
	//cmulmat(&P_WCE, (float) 200, &P_WCE);
	//float P_WCE_v[(ARMAX_dim*ARMAX_dim)+2];
	P_WCE.mtx = P_WCE_v;
	P_WCE.row = ARMAX_dim;
	P_WCE.col = ARMAX_dim;    
	P_WCE.dim = ARMAX_dim*ARMAX_dim;
    P_WCE_v[0] = SATRT_OF_VECTOR;
    P_WCE_v[(ARMAX_dim*ARMAX_dim)+1] = END_OF_VECTOR;
	for(i=1;i<=(ARMAX_dim*ARMAX_dim);i++)  P_WCE_v[i]=(float)0.0; 
	for(i=1; i<= ARMAX_dim;i++) P_WCE_v[i+((i-1)*ARMAX_dim)] = (float)1e3; 

	//loacal matrices:
	//matrix_alloc(&MtempA, ARMAX_dim, ARMAX_dim); //temporary matrix
	//float MtempA_v[(ARMAX_dim*ARMAX_dim)+2];
	MtempA.mtx = MtempA_v;
	MtempA.row = ARMAX_dim;
	MtempA.col = ARMAX_dim;    
	MtempA.dim = ARMAX_dim*ARMAX_dim;
    MtempA_v[0] = SATRT_OF_VECTOR;
    MtempA_v[(ARMAX_dim*ARMAX_dim)+1] = END_OF_VECTOR;
	for(i=1;i<=(ARMAX_dim*ARMAX_dim);i++)  MtempA_v[i]=(float)0.0; 

	//matrix_alloc(&MtempB, (ARMAX_dim, ARMAX_dim); //temporary matrix
	//float MtempB_v[(ARMAX_dim*ARMAX_dim)+2];
	MtempB.mtx = MtempB_v;
	MtempB.row = ARMAX_dim;
	MtempB.col = ARMAX_dim;    
	MtempB.dim = ARMAX_dim*ARMAX_dim;
    MtempB_v[0] = SATRT_OF_VECTOR;
    MtempB_v[(ARMAX_dim*ARMAX_dim)+1] = END_OF_VECTOR;
	for(i=1;i<=(ARMAX_dim*ARMAX_dim);i++)  MtempB_v[i]=(float)0.0; 

	// parameters for identification:
	delta = delta_WCE;
}

//-----------------------------------------------------------------------------
// Implementation:
//-----------------------------------------------------------------------------
/*
function [theta,v,P,delta] = WCE(psy,y_n,theta,P,delta,model)
%  Recoursive Worst CASE Estimation (WCE) algorithm 
%----------------------------------------------------------
%  Inputs:
% psy : [-y_n-1,...-y_n-ma,u_n-1,...u_n-mb,v_n-1,...v_n-mc]
%       vhere the y_xx index _xx is the time: t=T*xx 
% y_n : system output at time t=T*n 
% theta : etimated system paramethers at t=T*(n-1) (theta_n-1)
% P: 
% delta: maximum error max(v)
% m : model order [ma,mb,mc]
% 
% Outputs:
%   theta : etimated system paramethers 
%   v: residual in system identification
%
% For recursion: 
%   P : P_n-1 (estimated error covariance)
%   theta : theta_n-1 (estimated paramethers)
%
% Initialization:
%   P = eye(m(1,1)+m(1,2)+m(1,3)) * 10^22;
%   
%   theta =  zero matrix
%

g=psy'*P*psy;

v=y_n-psy'*theta;
v2=v^2;
%%%%% Insert
delta = delta*0.99;
delta = max(delta,abs(v)*1.9);
%%%%% End of insert
d=model(1,1)+model(1,2)+model(1,3)+model(1,4);

% Solution for ro -> maximum pozitive root of the equations
% (d-1)*g^2*ro^2-((2*d-1)*delta-g-v^2)*g*ro+delta*(d*(delta-v^2)-g)=0
% ro_1/2 = (-b +/- sqrt(b^2-4*a*c))/2a
% where a= (d-1)*g^2
%       b= ((2*d-1)*delta-g-v^2)*g
%       c= delta*(d*(delta-v^2)-g)
a= (d-1)*g^2;
b= ((2*d-1)*delta-g-v2)*g;
c= delta*(d*(delta-v2)-g);
% Numerical recepies in C, pp. 184:
b2_4ac= b^2-(4*a*c);
if b2_4ac >= 0
    q=(-1/2)*(b - sign(b) * sqrt(b2_4ac));
    ro_1=q/a;
    ro_2=c/q;
    ro=max([ro_1 ro_2]);
    if ro < 0
        ro=0;
    end
else
    ro=0;
end


% The gain term beta: 

beta = 1 + ro - ((ro*v2)/(delta^2 + ro * psy' * P * psy));

% The size and orientation of the elipsoid is

P = beta * (P - ((ro*P*psy*psy'*P)/((delta^2)+(ro*psy'*P*psy))));

% The center of the new elipsoid region become:

theta = theta + ((ro*P*psy*v)/((delta^2)+(ro*psy'*P*psy)));

*/

void WCE_alg::update(float y_n, float u_n_1)
{
	// y_n the systems output at time n*T (present time)

  float v2; // v2 = v_n * v_n;
  float g;  // g  = psy' * P * psy;
  float ge;	// =delta*delta + ro*(psy' * P * psy)
  float a;  // a= (d-1)*g^2;
  float b;  // b= ((2*d-1)*delta-g-v2)*g;
  float b_sgn; // =sgn(b)
  float c;  // c= delta*(d*(delta-v2)-g);
  float b2_4ac; // b2_4ac= b^2-(4*a*c);
  float q;
  float ro;	// ro_1=q/a;
  float beta; // beta = 1 + ro - ((ro*v2)/(delta^2 + ro * psy' * P * psy));

	update_psy(u_n_1); // control signal from the end of privious calculation 
//-----------------------------------------------------------------------------
	// Residual in system identification (predicion error):
	mulmat_t1(&MtempA, &psy, &theta);  // v = y_n - psy' * theta; 
	v_n = y_n - MtempA.mtx[1];		// Residual in system identification 
	v2 = v_n * v_n ; // residual power

//-----------------------------------------------------------------------------
	mulmat_t1(&MtempA, &psy, &P_WCE);  // g= psy'* P * psy ;
	mulmat(&MtempB, &MtempA, &psy);	
	g = MtempB.mtx[1];		///print_matrix(&MtempB,"g");

//-----------------------------------------------------------------------------
	 //% Solution for ro -> maximum pozitive root of the equations
	a = (ARMAX_dim-1)*(g*g);				 // a= (d-1)*g^2;
	b = ((2*ARMAX_dim-1)*delta-g-v2)*g;  // b= ((2*d-1)*delta-g-v2)*g;
	c = delta*(ARMAX_dim*(delta-v2)-g);  // c= delta*(d*(delta-v2)-g);

	b_sgn = (float)1.0; 
	if(b < 0.0) b_sgn = (float) -1.0;
	
	b2_4ac= (b*b) - (4*a*c);
	
	if (b2_4ac > (float)1e-9)  //-----------------------------------------------
	{
	       q = 0.5* (b + (b_sgn * sqrt(b2_4ac)));
	       ro = q/a;
	       b= c/q;
	       if(ro < b) ro = b; 
    

		  if (ro > 1e-8)
		  {
			//% The gain term beta:	// ge= delta^2 + ro(psy'* P * psy);
			ge = (delta * delta)+(ro * g);	
			//beta = 1 + ro - ((ro*v2)/(delta^2 + ro * psy' * P * psy));
			beta = 1 + ro - (ro*v2 / ge);

			//% The center of the new elipsoid region become:
			//theta = theta + ((ro*P*psy*v)/((delta^2)+(ro*psy'*P*psy)));
			mulmat(&MtempA, &P_WCE, &psy);	//MtempA = P*psy

			//= ro*P*psy*v/(delta^2 + ro*psy'*P*psy)
			cmulmat(&MtempA,((ro*v_n)/ge),&MtempA);
			addmat(&theta,&theta,&MtempA);	//theta  <- the new

			//% The size and orientation of the elipsoid is	
			//P = beta * (P - ((ro*P*psy*psy'*P)/((delta^2)+(ro*psy'*P*psy))));	
			mulmat_t1(&MtempA, &psy, &P_WCE);	//MtempA = psy'*P
			mulmat(&MtempB, &psy, &MtempA);		//MtempB = psy*psy'*P
			mulmat(&MtempA, &P_WCE, &MtempB);	//MtempA = P*psy*psy'*P
			cmulmat(&MtempA,ro/ge,&MtempA);		//MtempA = ro*P*psy*psy'*P
			submat(&P_WCE,&P_WCE,&MtempA);		//P_WCE  = P - ((ro*P*psy*psy'*P
			cmulmat(&P_WCE,beta,&P_WCE);	//P_WCE  <- the new			
		  }	
	   
	}	//---------------------------------------------------------------------


	update_psy(y_n, v_n); // Updates the psy vector with time shift

	// Statistic calculation for the system identification:
    if(update_stat() == 1 ) // The results are redy:
	{
		count_n = NxT; // Start the next statistical evaluation
	}

}


//-----------------------------------------------------------------------------
// DeInitialisation:
//-----------------------------------------------------------------------------

WCE_alg::~WCE_alg()  // deInitialization
{

}

//-----------------------------------------------------------------------------