/*
 *  Copyright 2004 by Soos, Antal 
 *  All rights reserved. Property of Soos, Antal. 
 *  Restricted rights to use, duplicate or disclose this code are 
 *  granted through contract.   
 */ 
/***************************************************************************/ 
/*                                                                         */ 
/*     H8_MPC . hpp		                                           */ 
/*                                                                         */ 
/*     H8 Model Predictive Control Control Algorithm Inplementation.       */ 
/*									   */ 
/*                                                                         */ 
/***************************************************************************/ 


#ifndef _H8MPC_HPP_ 
#define _H8MPC_HPP_ 

#include "sysContr.hpp" 


#define GAMA_TOL 0.1
#define OK    0
#define FAIL  1
#define RETUNE 24

class H8MPC_alg : public syscr { 
 
        int sys_l;      // # of system disturbances 
        int sys_r;	// # of system control input 
        int sys_p;	// # of system controlled output 
        int sys_q;	// # of system measured outputs 
        int sys_m;	// control system_dimension = m_a + m_b; 
  
        float gama_0;  // optimal solution for gama
        
        float fy__ii;
       
        int flip_flag;
 
        float yi_1 ;    // H8 control input filter 
        float yi_2 ;
        float yi_3 ;
        float yi_4 ;
        float yi_5 ;

        float yo_1 ;    // H8 control output filter 
        float yo_2 ;
        float yo_3 ;
        float yo_4 ;
        float yo_5 ;
        
        int newsystem;
        float y_i_vector[RETUNE];
        float u_i_vector[RETUNE];

	// System matrice for the state space model: 
        Matrix mA;    //   x__np = mA * x__n + mB * u__n + mD * w__n
	Matrix mB;
	Matrix mD;
	Matrix mC;    //   y__n = mC * x__n + mE * w__n 
        Matrix mE;    
	Matrix mH;    //   z__n = mH * x__n + mG * u__n 
        Matrix mG;	
	
        Matrix mQ;      // Q = H'*H
        Matrix mS;      // S = H'*G
        Matrix mR;      // R = G'*G
        Matrix mR_inv;  // R_inv = inv(R)

        Matrix mW;      // W = D*D'
        Matrix mL;      // L = D*E'
        Matrix mV;      // V = E*E'
        Matrix mV_inv;  // V_inv = inv(V)

        Matrix mA_bar;   // A_bar   = A - B*R_inv*S'  
        Matrix mQ_bar;   // Q_bar   = Q - S*R_inv*S'
        Matrix mRinvS;   // mRinvS  = R_inv*S'

        Matrix mA_tilde; // A_tilde = A - L*V_inv*C
        Matrix mW_tilde; // W_tilde = W - L*V_inv*L'
        Matrix mLVinv;   // mLVinv  = L*V_inv

        Matrix mP1tar;  // first Riccati recursion result
        Matrix mP2tar;  // secound Riccati recursion result  
        Matrix mP1;     // first Riccati recursion
        Matrix mP2;     // secound Riccati recursion
 
        Matrix mP1_inv; // first Riccati recursion
        Matrix mP2_inv; // secound Riccati recursion

        Matrix mP1_tilde;  // Test for Riccati 1
        Matrix mP2_tilde;  // Test for Riccati 2



         Matrix mBD;  // [B D]   
        // [R + B'*P1*B  B'*P1*D; D'*P1*B  -gama2 * eye(l) + D'*P1*D ]
        Matrix mGama_inv_C;  // Gama_inv_C = inv(Gama__C);
        Matrix mGDPD;  // -gama2 * eye(l) + D'*P1*D 

        
        Matrix mCH;  // [C; H]  
        // [V + C*P2*C'  C*P2*H'; H*P2*C'  -gama2 * eye(p) + H*P2*H' ]
        Matrix mGama_inv_F;  // inv(Gama__F);
        Matrix mGHPH; // -gama2 * eye(p) + H*P2*H'

       // controller
        Matrix mKcx;
        Matrix mKcy;

        // prediction
        Matrix mKfx;
        Matrix mKfu;
        Matrix mKfy;



        Matrix mT1;      // temporary matrix 
        Matrix mT2;      // temporary matrix 
        Matrix mT3;      // temporary matrix
        Matrix mT4;      // temporary matrix 


        Matrix mru;       // real-time matrix system output vector
        Matrix mry;       // real-time matrix system input vector
        Matrix mrx;       // real-time matrix system state
      
        Matrix mrT1;      // real-time temporary matrix 
        Matrix mrT2;      // real-time temporary matrix 

	 
public: 
 
    void update(Matrix *psy, float gama); 
    float rtm_control(float fy__i, float fu__i); 
    float rtm_update(float fy__i, float fu__i); 
    H8MPC_alg( int is,int ia,int ib,int ic,int iq,int ip,int it,int ir); 
    int   weightSet();
    ~H8MPC_alg(); 
}; 
 
 
#endif // _H8MPC_HPP_