#include "math.h"
#include "stdlib.h"
#define Hp (30) /*prediction Horizon*/
#define Hu (10) /*controll Horizon*/
#define Hw (1) /*window parameter*/
#define TS (0.004) /*sample Time*/
#define TARGET (0.0)
/*weight of Fcn*/
#define Wq (5000)
#define Wr (1)
#define Tref (0.1) /*応答の速さの時定数*/
/*int TimecounterMPC = 0;*/
/*counter*/
int i,j,k,i1;
int i5,j5,k5;
int jadge ;
/*model parameter*/
/* raw41 parameter*/
#define RPb_Rne (3.996)
#define RPb_Rth (67.35)
#define T_pm (0.3)
#define Rga_RPb (0.03077)
#define RTe_Rga (0.2050)
#define RTe_Rgf (-1.25)
#define J (0.3385)
#define RTL_Rne (0.296)
#define alpha (0.5606)
#define T_ff (0.104)
#define T_s (0.2)
#define Rgf_Rtf (-0.0003029)
#define T_fb (0.05165)
#define Rlo_Rga (0.465) /*RAF_Rga*/
#define Rlo_Rgf (0.8768) /*RAF_Rgf*/
double A[5][5];//Ad^n等
double AA[5][5];//Ad^n等
double AA2[5][5];//Ad^n等
double B[5][1];//Bd
double Bd[5][1];//Bdd
double C[Hp-Hw+1][5*(Hp-Hw+1)];//;bigC
/*difinition of each array*/
double Psi[5*(Hp-Hw+1)][5] ;//Psi
double CPsi[(Hp-Hw+1)][5];//C*Psi
double CPsix[(Hp-Hw+1)][1];//C*Psi*x
double Ups[5*(Hp-Hw+1)][1] ;
double Ups1Hp[5*Hp][1];/*1110*/
double CUps[(Hp-Hw+1)][1];
double Xi[5*(Hp-Hw+1)][Hp];
double SHADBd[5][1];//5*1行列の入れ物
double Xiline[5*Hp][1];//Xiの１行目
double Xi1Hp[5*Hp][5*Hp];/*1110*/
double CXi[Hp-Hw+1][Hp];
double CXidm0[Hp-Hw+1][1];
double CXidm1[Hp-Hw+1][1];
double CXidm[Hp-Hw+1][1];
double The[5*(Hp-Hw+1)][Hu] ;
double The1Hp[5*Hp][Hu];/*1110*/
double CTHE[Hp-Hw+1][Hu];
double Tau[Hp-Hw+1][1] ;
double tracer[Hp-Hw+1][1] ; /*reference trajectory*/
double upast[2] ;//u(k-1)
double Setpoint[Hp-Hw+1][1] ;//目標値s
double ERROR[Hp-Hw+1][1];//プラント,モデルとの出力誤差
double Yfree[Hp-Hw+1][1];
double Yfree1[Hp][1];
double THET[Hu][5*(Hp-Hw+1)];
double CT[5*(Hp-Hw+1)][Hp-Hw+1];//Cの転置行列
double THETCT[Hu][Hp-Hw+1];
double THETCTQ[Hu][Hp-Hw+1];
double THETCTQC[Hu][5*(Hp-Hw+1)];
double THETCTQCTHE[Hu][Hu];
/*Weight array*/
double Q[Hp-Hw+1][Hp-Hw+1] ;
double R[Hu][Hu] ;
double ipfirst ;//初期 sw
double ip ;
double SHAD[5][5]; /*Psi calculation*/
double SHAD1[5][1];/*Ups calculation*/
double SHAD2[5][5];
double SHAD3[5][1];
double SHAD4[5][5];
double SHAD5[5][5];
double Deltau[Hu][1];
double deltau ;
double H[Hu][Hu];
double HT[Hu][Hu];//H^-1
double G[Hu][1];
/*use for calcilating inverse*/
/*size of array*/
double buf;
double buf1 ;
double buf2 ;
void mdlInitializeConditions(SimStruct *S) /*状態の初期値設定*/ //*S[x1, ,x5,error,y,dm] １周期目の関数,
{
//Ad^n等
A[0][0]=-(1/J)*RTL_Rne*TS+1;
A[0][1]=(1/J)*(Rga_RPb)*(RTe_Rga)*TS;
A[0][2]=0;
A[0][3]=(1/J)*RTe_Rgf*TS ;
A[0][4]=0;
A[1][0]=-(1/T_pm)*(RPb_Rne)*TS;
A[1][1]=-(1/T_pm)*TS+1;
A[1][2]=0;
A[1][3]=0;
A[1][4]=0;
A[2][0]=0;
A[2][1]=(1/T_s)*(Rga_RPb)*(Rlo_Rga)*TS;
A[2][2]=-(1/T_s)*TS+1;
A[2][3]=(1/T_s)*(Rlo_Rgf)*TS;
A[2][4]=0;
A[3][0]=0;
A[3][1]=0;
A[3][2]=0;
A[3][3]= -(1/T_fb)*TS+1 ;
A[3][4]=(1/T_fb)*TS;
A[4][0]=0;
A[4][1]=0;
A[4][2]=0;
A[4][3]=0;
A[4][4]= -(1/T_ff)*TS+1;
//Ad^n等
AA[0][0]=-(1/J)*RTL_Rne*TS+1;
AA[0][1]=(1/J)*(Rga_RPb)*(RTe_Rga)*TS;
AA[0][2]=0;
AA[0][3]=(1/J)*RTe_Rgf*TS ;
AA[0][4]=0;
AA[1][0]=-(1/T_pm)*(RPb_Rne)*TS;
AA[1][1]=-(1/T_pm)*TS+1;
AA[1][2]=0;
AA[1][3]=0;
AA[1][4]=0;
AA[2][0]=0;
AA[2][1]=(1/T_s)*(Rga_RPb)*(Rlo_Rga)*TS;
AA[2][2]=-(1/T_s)*TS+1;
AA[2][3]=(1/T_s)*(Rlo_Rgf)*TS;
AA[2][4]=0;
AA[3][0]=0;
AA[3][1]=0;
AA[3][2]=0;
AA[3][3]= -(1/T_fb)*TS+1 ;
AA[3][4]=(1/T_fb)*TS;
AA[4][0]=0;
AA[4][1]=0;
AA[4][2]=0;
AA[4][3]=0;
AA[4][4]= -(1/T_ff)*TS+1;
//Ad
AA2[0][0]=-(1/J)*RTL_Rne*TS+1;
AA2[0][1]=(1/J)*(Rga_RPb)*(RTe_Rga)*TS;
AA2[0][2]=0;
AA2[0][3]=(1/J)*RTe_Rgf*TS ;
AA2[0][4]=0;
AA2[1][0]=-(1/T_pm)*(RPb_Rne)*TS;
AA2[1][1]=-(1/T_pm)*TS+1;
AA2[1][2]=0;
AA2[1][3]=0;
AA2[1][4]=0;
AA2[2][0]=0;
AA2[2][1]=(1/T_s)*(Rga_RPb)*(Rlo_Rga)*TS;
AA2[2][2]=-(1/T_s)*TS+1;
AA2[2][3]=(1/T_s)*(Rlo_Rgf)*TS;
AA2[2][4]=0;
AA2[3][0]=0;
AA2[3][1]=0;
AA2[3][2]=0;
AA2[3][3]= -(1/T_fb)*TS+1 ;
AA2[3][4]=(1/T_fb)*TS;
AA2[4][0]=0;
AA2[4][1]=0;
AA2[4][2]=0;
AA2[4][3]=0;
AA2[4][4]= -(1/T_ff)*TS+1;
//Bd
B[0][0]=0;
B[1][0]=0;
B[2][0]=0;
B[3][0]=alpha*(1/T_fb)*(Rgf_Rtf)*TS;
B[4][0]=(1-alpha)*(1/T_ff)*(Rgf_Rtf)*TS;
//Bdd
Bd[0][0]=0;
Bd[1][0]=(1/T_pm)*RPb_Rth*TS ;
Bd[2][0]=0;
Bd[3][0]=0;
Bd[4][0]=0;
/*CALCULATING MATRIX Psi start*/
for (i1=1;i1<=Hp;i1++){
for(i=0;i<=4;i++){
for(j=0;j<=4;j++){
SHAD[i][j] = AA[i][j];
}
}
if(i1 >= Hw){
for(i=0;i<=4;i++){
for(j=0;j<=4;j++){
Psi[5*(i1-Hw)+i][j] = SHAD[i][j] ;
}
}
}
for (i=0;i<=4;i++){
for (j=0;j<=4;j++){
SHAD[i][j] = 0;
for(k=0;k<=4;k++){
SHAD[i][j] += A[i][k] * AA[k][j];
}
}
}
for(i=0;i<=4;i++){
for(j=0;j<=4;j++){
AA[i][j]=SHAD[i][j];
}
}
}
/*CALCULATING MATRIX Psi end*/
/*CALCULATING MATRIX Ups start*/
for(i=0;i<=4;i++){
SHAD1[i][0]=B[i][0];
}
for (i1=0;i1<=Hp-1;i1++){
for(i=0;i<=4;i++){
for(j=0;j<=4;j++){
SHAD2[i][j] = AA2[i][j];
}
}
for(i=0;i<=4;i++){
Ups1Hp[5*i1+i][0] = SHAD1[i][0] ;
}
for(i=0;i<=4;i++){
SHAD3[i][0] = 0;
for(j=0;j<=4;j++){
SHAD3[i][0] += SHAD2[i][j]*B[j][0];
}
}
/*SHAD1=Ai*B*/
for(i=0;i<=4;i++){
SHAD1[i][0] += SHAD3[i][0];
}
for (i=0;i<=4;i++){
for (j=0;j<=4;j++){
SHAD2[i][j] = 0;
for(k=0;k<=4;k++){
SHAD2[i][j] += A[i][k] * AA2[k][j];
}
}
}
for(i=0;i<=4;i++){
for(j=0;j<=4;j++){
AA2[i][j] = SHAD2[i][j];
}
}
}
/*make Ups*/
for(i=0;i<=5*(Hp-Hw+1)-1;i++){
Ups[i][0] = Ups1Hp[5*(Hw-1)+i][0];
}
/*CALCULATING MATRIX Ups end*/
/*CALCULATING MATRIX The & THET start*/
/*THETA initialization by 0*/
for(i=0;i<=5*Hp-1;i++){
for(j=0;j<=Hu-1;j++){
The1Hp[i][j] = 0 ;
}
}
for(i=0;i<=Hu-1;i++){
for(j=0;j<=5*Hp-1;j++){
if(j+5*i <= 5*Hp-1){
The1Hp[j+5*i][i] = Ups1Hp[j][0];
}
else{
;
}
}
}
/*make The (cut The1Hp between Hw~Hp*/
for(i=0;i<=5*(Hp-Hw+1)-1;i++){
for(j=0;j<=Hu-1;j++){
The[i][j] = The1Hp[5*(Hw-1)+i][j];
}
}
for(i = 0; i <=Hu-1 ; i++){
for(j = 0 ; j <=5*(Hp-Hw+1)-1 ; j++){
THET[i][j] = The[j][i] ;
}
}
/*CALCULATING MATRIX The & THET end*/
/*MAKING MATRIX Xi start*/
/*shad change to E*/
for (i=0;i<=4;i++){
for (j=0;j<=4;j++){
if(i==j){
SHAD4[i][j] = 1;
}
else{
SHAD4[i][j] = 0;
}
}
}
for(i1=0;i1<=Hp-1;i1++){
for(i=0;i<=4;i++){
SHADBd[i][0] = 0;
for(j=0;j<=4;j++){
SHADBd[i][0] += SHAD4[i][j] * Bd[j][0];
}
}
for(i=0;i<=4;i++){
Xiline[5*i1+i][0] = SHADBd[i][0];
}
for(i=0;i<=4;i++){
for(j=0;j<=4;j++){
SHAD5[i][j] = 0;
for(k=0;k<=4;k++){
SHAD5[i][j] += SHAD4[i][k] * A[k][j];
}
}
}
for(i=0;i<=4;i++){
for(j=0;j<=4;j++){
SHAD4[i][j] = SHAD5[i][j];
}
}
}
/*Xi1Hp initialization by 0*/
for(i=0;i<=5*Hp-1;i++){
for(j=0;j<=Hp-1;j++){/*1110*/
Xi1Hp[i][j] = 0 ;
}
}
/*Hp Hp array Xi1Hp make*/
for(i=0;i<=Hp-1;i++){
for(j=0;j<=5*Hp-1;j++){
if(j+5*i <= 5*Hp-1){
Xi1Hp[j+5*i][i] = Xiline[j][0];
}
else{
;
}
}
}
/*cut (Hp-Hw+1)Hp*/
for(i=0;i<=5*(Hp-Hw+1)-1;i++){
for(j=0;j<=Hp-1;j++){
Xi[i][j] = Xi1Hp[i+5*(Hw-1)][j];
}
}
/*MAKING MATRIX Xi end*/
/*CALCULATING MATRIX C & CT start*/
for(i=0;i<=Hp-Hw;i++){
for(j=0;j<=5*(Hp-Hw+1)-1;j++){
C[i][j] = 0 ;
}
}
for(i=0;i<=Hp-Hw;i++){
C[i][5*i+2] = 1 ;
}
for(i=0;i<=5*(Hp-Hw+1)-1;i++){
for(j=0;j<=Hp-Hw+1-1;j++){
CT[i][j] = C[j][i];
}
}
/*CALCULATING
