/*
 *  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 5
#define gama_i  1.0 

class H8MPC_alg : public syscr { 
 
protected:
  		float gama;  // search holder for gama
        float gama_0;  // optimal solution for gama

        //float fy__ii;
       
        int flip_flag;
         
        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
		float mA_v[(H8_m*H8_m)+2];
		Matrix mB;
		float mB_v[(H8_m*H8_r)+2];
		Matrix mD;
		float mD_v[(H8_m*H8_l)+2];
		Matrix mC;    //   y__n = mC * x__n + mE * w__n 
		float mC_v[(H8_q*H8_m)+2];
        Matrix mE;    
		float mE_v[(H8_q*H8_l)+2];
		Matrix mH;    //   z__n = mH * x__n + mG * u__n 
		float mH_v[(H8_p*H8_m)+2];
        Matrix mG;	
		float mG_v[(H8_p*H8_r)+2];
        Matrix mQ;      // Q = H'*H
		float mQ_v[(H8_m*H8_m)+2];
        Matrix mS;      // S = H'*G
		float mS_v[(H8_m*H8_r)+2];
        Matrix mR;      // R = G'*G
		float mR_v[(H8_r*H8_r)+2];
        Matrix mR_inv;  // R_inv = inv(R)
		float mR_inv_v[(H8_r*H8_r)+2];

        Matrix mW;      // W = D*D'
		float mW_v[(H8_m*H8_m)+2];
        Matrix mL;      // L = D*E'
		float mL_v[(H8_m*H8_q)+2];
        Matrix mV;      // V = E*E'
		float mV_v[(H8_q*H8_q)+2];
        Matrix mV_inv;  // V_inv = inv(V)
		float mV_inv_v[(H8_q*H8_q)+2];

        Matrix mA_bar;   // A_bar   = A - B*R_inv*S'  
		float mA_bar_v[(H8_m*H8_m)+2];
        Matrix mQ_bar;   // Q_bar   = Q - S*R_inv*S'
		float mQ_bar_v[(H8_m*H8_m)+2];
        Matrix mRinvS;   // mRinvS  = R_inv*S'
		float mRinvS_v[(H8_r*H8_m)+2];

        Matrix mA_tilde; // A_tilde = A - L*V_inv*C
		float mA_tilde_v[(H8_m*H8_m)+2];
        Matrix mW_tilde; // W_tilde = W - L*V_inv*L'
		float mW_tilde_v[(H8_m*H8_m)+2];
        Matrix mLVinv;   // mLVinv  = L*V_inv
		float mLVinv_v[(H8_m*H8_m)+2];

        Matrix mP1tar;  // first Riccati recursion result
		float mP1tar_v[(H8_m*H8_m)+2];
        Matrix mP2tar;  // secound Riccati recursion result 
		 	float mP2tar_v[(H8_m*H8_m)+2];
        Matrix mP1;     // first Riccati recursion
		float mP1_v[(H8_m*H8_m)+2];
        Matrix mP2;     // secound Riccati recursion
		float mP2_v[(H8_m*H8_m)+2];
 
        Matrix mP1_inv; // first Riccati recursion
 		float mP1_inv_v[(H8_m*H8_m)+2];
        Matrix mP2_inv; // secound Riccati recursion
		float mP2_inv_v[(H8_m*H8_m)+2];

        Matrix mP1_tilde;  // Test for Riccati 1
		float mP1_tilde_v[(H8_m*H8_m)+2];
        Matrix mP2_tilde;  // Test for Riccati 2
		float mP2_tilde_v[(H8_m*H8_m)+2];



         Matrix mBD;  // [B D]   
		float mBD_v[(H8_m*(H8_r+H8_l))+2];
        // [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);
		float mGama_inv_C_v[((H8_r+H8_l)*(H8_r+H8_l))+2];
        Matrix mGDPD;  // -gama2 * eye(l) + D'*P1*D 
		float mGDPD_v[(H8_l*H8_l)+2];

        
        Matrix mCH;  // [C; H]  
        // [V + C*P2*C'  C*P2*H'; H*P2*C'  -gama2 * eye(p) + H*P2*H' ]
		float mCH_v[((H8_q+H8_p)*H8_m)+2];
	    Matrix mGama_inv_F;  // inv(Gama__F);
		float mGama_inv_F_v[((H8_q+H8_p)*(H8_q+H8_p))+2];
        Matrix mGHPH; // -gama2 * eye(p) + H*P2*H'
		float mGHPH_v[(H8_p*H8_p)+2];

       // controller
        Matrix mKcx;
		float mKcx_v[(H8_r*H8_m)+2];
        Matrix mKcy;
		float mKcy_v[(H8_r*H8_q)+2];

        // prediction
        Matrix mKfx;
		float mKfx_v[(H8_m*H8_m)+2];
        Matrix mKfu;
		float mKfu_v[(H8_m*H8_r)+2];
        Matrix mKfy;
		float mKfy_v[(H8_m*H8_q)+2];


      // Real-time controller
        Matrix mrtKcx;
		float mrtKcx_v[(H8_r*H8_m)+2];
        Matrix mrtKcy;
		float mrtKcy_v[(H8_r*H8_q)+2];

        //  Real-time prediction
        Matrix mrtKfx;
		float mrtKfx_v[(H8_m*H8_m)+2];
        Matrix mrtKfu;
		float mrtKfu_v[(H8_m*H8_r)+2];
        Matrix mrtKfy;
		float mrtKfy_v[(H8_m*H8_q)+2];


        Matrix mT1;      // temporary matrix 
		float mT1_v[((H8_p+H8_q)*(H8_p+H8_q))+2];
        Matrix mT2;      // temporary matrix 
		float mT2_v[((H8_p+H8_q)*(H8_p+H8_q))+2];
        Matrix mT3;      // temporary matrix
		float mT3_v[((H8_p+H8_q)*(H8_p+H8_q))+2];
        Matrix mT4;      // temporary matrix 
		float mT4_v[((H8_p+H8_q)*(H8_p+H8_q))+2];
	

        Matrix mru;       // real-time matrix system output vector
		float mru_v[(H8_r*1)+2];
        Matrix mry;       // real-time matrix system input vector
		float mry_v[(H8_q*1)+2];
        Matrix mrx;       // real-time matrix system state
		float mrx_v[(H8_m*1)+2];
      
        Matrix mrT1;      // real-time temporary matrix 
		float mrT1_v[(H8_m*H8_m)+2];
        Matrix mrT2;      // real-time temporary matrix 
		float mrT2_v[(H8_m*H8_m)+2];

	 
public: 
 
       H8MPC_alg(float notused); 
		~H8MPC_alg(void); 
	   void update(Matrix *psy); 
    float rtm_control(float fy__i, float fu__i); 
    float rtm_update(float fy__i, float fu__i); 
    void   weightSet(void);
   float get_gama(void); // the H8 gama  
   
    float rtm_PID_control(float fy__i, float fu__i);  
}; 
 
 
#endif // _H8MPC_HPP_