/* Decision Table [5][5] -> 0.1
   py_pe_table[][] and py_pce_table[][] are also modified */
#include <stdio.h>
#include <math.h>
#include   <string.h>
#include   <stdlib.h>
#include   <alloc.h>
#define TRUE 1
#define FALSE 0
#define SPACE 0.2

float decision_t[11][11]={
{-0.0000, 0.0714, 0.1526, 0.2996, 0.4706, 0.5612, 0.5700, 0.6757, 0.8203, 0.8383, 0.8504},
{-0.0714, 0.0000, 0.0904, 0.2415, 0.4393, 0.4706, 0.5175, 0.6600, 0.7547, 0.7600, 0.8383},
{-0.1526,-0.0904,-0.0000, 0.1216, 0.2415, 0.3388, 0.5000, 0.5608, 0.5700, 0.7547, 0.8203},
{-0.2996,-0.2415,-0.1216,-0.0000, 0.0904, 0.2629, 0.3993, 0.4100, 0.5608, 0.6600, 0.6757},
{-0.4706,-0.4393,-0.2415,-0.0904, 0.0000, 0.0887, 0.1959, 0.3993, 0.5000, 0.5175, 0.5700},
{-0.5300,-0.4706,-0.3388,-0.2629,-0.0887, 0.1, 0.0887, 0.2629, 0.3388, 0.4706, 0.5612},
{-0.5607,-0.5175,-0.5000,-0.3993,-0.1959,-0.0887, 0.0000, 0.0904, 0.2415, 0.4393, 0.4706},
{-0.6900,-0.6768,-0.5608,-0.4100,-0.3993,-0.2629,-0.0904,-0.0000, 0.1216, 0.2415, 0.2996},
{-0.8203,-0.7547,-0.5700,-0.5608,-0.5000,-0.3388,-0.2415,-0.1216,-0.0000, 0.0904, 0.1526},
{-0.8383,-0.7600,-0.7547,-0.6768,-0.5175,-0.4706,-0.4393,-0.2415,-0.0904, 0.0000, 0.0714},
{-0.8504,-0.8383,-0.8203,-0.6900,-0.5607,-0.5300,-0.4706,-0.2996,-0.1526,-0.0714,-0.0000}};

float py_pe_t[11][11]={
{ 0.3571, 0.4060, 0.7350, 0.8547, 0.4534, 0.0438, 0.5285, 0.7232, 0.0897, 0.0608, 0.4789},
{ 0.3571, 0.4519, 0.7555, 0.9889, 0.1565, 0.2347, 0.7125, 0.4735, 0.0264, 0.3914, 0.3859},
{ 0.3113, 0.4519, 0.6082, 0.5992, 0.4864, 0.8062, 0.3042, 0.0458, 0.9236, 0.3281, 0.0483},
{ 0.2907, 0.5992, 0.6082, 0.4519, 0.8628, 0.6820, 0.0533, 0.7542, 0.4958, 0.0784, 0.3927},
{ 0.1565, 0.9889, 0.7555, 0.4519, 0.4433, 0.5360, 1.0174, 0.5033, 0.0875, 0.2625, 0.5184},
{ 0.2972, 0.6590, 0.3791, 0.8714, 0.943, - 0.0565, 0.8714, 0.3791, 0.6590, 0.4534, 0.0438},
{ 0.2162, 0.0875, 0.5033, 1.0174, 0.5360, 0.4433, 0.4519, 0.7555, 0.9889, 0.1565, 0.1472},
{ 0.0659, 0.5799, 0.7542, 0.0533, 0.6820, 0.8628, 0.4519, 0.6082, 0.5992, 0.2907, 0.5682},
{ 0.3281, 0.9236, 0.0458, 0.3042, 0.8062, 0.4864, 0.5992, 0.6082, 0.4519, 0.3113, 0.4970},
{ 0.3914, 0.0264, 0.3894, 0.7967, 0.2347, 0.1565, 0.9889, 0.7555, 0.4519, 0.3571, 0.4747},
{ 0.0608, 0.0897, 0.6517, 0.6464, 0.1537, 0.2972, 0.8547, 0.7350, 0.4060, 0.3571, 0.5996}};

float py_pce_t[11][11]={
{-0.3571,-0.3571,-0.3113,-0.2907,-0.1565,-0.4534,-0.2625,-0.0784,-0.3281,-0.3914, 0.0000},
{-0.4060,-0.4519,-0.4519,-0.5992,-0.9889,-0.6590,-0.0875,-0.4958,-0.9236,-0.0264, 0.0000},
{-0.7350,-0.7555,-0.6082,-0.6082,-0.7555,-0.3791,-0.5033,-0.7542,-0.0458,-0.4735, 0.0000},
{-0.8547,-0.9889,-0.5992,-0.4519,-0.4519,-0.8714,-1.0174,-0.0533,-0.3042,-0.7125, 0.0000},
{-0.2972,-0.1565,-0.4864,-0.8628,-0.4433, 0.0565,-0.5360,-0.6820,-0.8062,-0.2347, 0.0000},
{-0.1537,-0.2347,-0.8062,-0.6820,-0.5360,-0.943,-0.4433,-0.8628,-0.4864,-0.1565, 0.0000},
{-0.6464,-0.7967,-0.3042,-0.0533,-1.0174,-0.8714,-0.4519,-0.4519,-0.5992,-0.9889, 0.0000},
{-0.6517,-0.3894,-0.0458,-0.7542,-0.5033,-0.3791,-0.7555,-0.6082,-0.6082,-0.7555, 0.0000},
{-0.0897,-0.0264,-0.9236,-0.5799,-0.0875,-0.6590,-0.9889,-0.5992,-0.4519,-0.4519, 0.0000},
{-0.0608,-0.3914,-0.3281,-0.0659,-0.2162,-0.2972,-0.1565,-0.2907,-0.3113,-0.3571, 0.0000},
{-0.4789,-0.3859,-0.0483,-0.3212,-0.5647,-0.0732,-0.1472,-0.5682,-0.4970,-0.4747, 0.0000}};


int PASS;
float dd=0.01;
float th=0.02;
float yr, yu, yu_true, y_plant, ydot, y_plant_dd;
float py_pe,py_pce;
float e, e_old, ce;
float s1, s2, s3, es1, ces2;
float ds1, ds2, ds3;
float out_t;
float u_plant,u_plant_dd;
float eta_s1, alpha_s1, eta_s2, alpha_s2, eta_s3, alpha_s3;
float delta4, delta3, delta21, delta22;
float ce_down, ce_up, e_left, e_right;
float du;
float x1,x1_old,x2,x2_old;
float errorsum;

int k_index, pass, te;
int qe_t, qce_t;
int qce_down, qce_up, qe_left, qe_right;

FILE *fscalor,*ferror,*foutput,*fl_curve,*fplant_u,*fscalor_in;
char scalor[20],error[20],output[20],l_curve[20],plant_u[20],scalor_in[20];
void initialize();
void rmodel();
void adjust_s1s2();
int quantize(float);
void forward_plant();
float plant1(float,float);
float plant2(float,float);
void backward_c();
void corner();
float ctrl_o_decide();
float py_pe_decide();
float py_pce_decide();
float iterpolation(float,float,float);
void op_file_def();
void s1s2_adjust();
void main (void)
{
 int i,j,k;
 int pseudo;
 float temp;

 te=1001;
 temp=(float)(te);
 op_file_def();
 initialize();
 printf("s1=%f s2=%f s3=%f eta_s1=%f alpha_s1=%f",s1,s2,s3,eta_s1,alpha_s1);
 printf(" eta_s2=%f alpha_s2=%f eta_s3=%f alpha_s3=%f\n",eta_s2,alpha_s2,eta_s3,alpha_s3);
 while(pass<=PASS)
 {
/*  fprintf(fscalor,"s1(%d)=%f; s2(%d)=%f; s3(%d)=%f;\n",pass,s1,pass,s2,pass,s3);*/
  printf("pass=%d s1=%f s2=%f s3=%f\n",pass,s1,s2,s3);
  while(k_index<=te)
   {
    rmodel();
/*    if(pass==1)
      {
        fprintf(foutput,"y_before(%d)=%f;\n",k_index,y_plant);
        fprintf(fplant_u,"plant_u_b(%d)=%f;\n",k_index,u_plant);
      }   */
      if(pass==PASS)
     {
     /*  fprintf(ferror,"e(%d)=%f; ce(%d)=%f;\n",k_index,e,k_index,ce); */
       fprintf(foutput,"y_after(%d)=%f;\n",k_index,y_plant);
/*       fprintf(fplant_u,"plant_u_a(%d)=%f;\n",k_index,u_plant); */
     }

    e_old=e;
    e=yr-y_plant;         /* e(k)= sp(k)- y(k) */
    ce= -(e-e_old);       /* ce(k)=e(k-1)-e(k)= -( e(k)-e(k-1) ) */

    s1s2_adjust();
    if(pass==PASS)
     {
      printf("\npass=%d k_index=%d\n",pass,k_index);
      printf("s1=%f s2=%f s3=%f",s1,s2,s3);
      fprintf(fscalor_in,"s1(%d)=%f; s2(%d)=%f; s3(%d)=%f;\n",k_index,s1,k_index,s2,k_index,s3);
     }
    corner();        /* decide e_left, e_right, ce_up, ce_down, qe_left, qe_right, qce_up, qce_down */

    out_t=ctrl_o_decide();
    py_pe=py_pe_decide();
    py_pce=py_pce_decide();

    u_plant=out_t*s3;
    forward_plant();

    if(pass!=1)
        backward_c();
    errorsum+=(yr-y_plant)*(yr-y_plant);
    if(pass==PASS)
     {
       printf(" yr=%f e=%f ce=%f es1=%f ces2=%f",yr,e,ce,es1,ces2);
       printf(" qce_u=%d qe_l=%d out_t=%f",qce_up,qe_left,out_t);
/*       printf(" ds1=%f ds2=%f ds3=%f",ds1,ds2,ds3); */
/*       printf(" d4=%f d3=%f d21=%f d22=%f",delta4,delta3,delta21,delta22); */
       printf(" u_plant=%f yu=%f y_plant=%f\n",u_plant,yu,y_plant);
     }
    k_index++;
   }  /*-------- END OF INNER WHILE LOOP ----------*/
  printf("   errorsum=%f MSE=%f\n",errorsum,errorsum/temp);
  fprintf(fl_curve,"lc(%d)=%f;\n",pass,errorsum/temp);
  pass++;
  errorsum=0.0;
  yr=4.0;
  y_plant=ydot=0.0;
  e=e_old=yr;
  u_plant=0.0;
  x1_old=x1=x2=x2_old=0.0;

  k_index=1;
  }  /*------- END OF OUTER WHILE LOOP ----------*/
  fclose(fscalor);
  fclose(ferror);
  fclose(foutput);
  fclose(fl_curve);
  fclose(fplant_u);
  fclose(fscalor_in);
}

/*--------------------- Parameter Initialization ----------------------------*/
void initialize(void)
{
  pass=1;
  yr=4.0;
  y_plant=ydot=0.0;
  e=e_old=yr;
  u_plant=0.0;
  x1_old=x1=x2=x2_old=0.0;
  errorsum=0.0;

  ds1=ds2=ds3=0.0;

  k_index=1;

    printf("Please input the value of s1:");
    scanf("%f",&s1);
    printf("\nPlease input the value of s2:");
    scanf("%f",&s2);
    printf("\nPlease input the value of s3:");
    scanf("%f",&s3);

  printf("\nPlease input the value of learning 'eta_s1':");
  scanf("%f",&eta_s1);
  printf("\nPlease input the value of learning 'alpha_s1':");
  scanf("%f",&alpha_s1);
  printf("\nPlease input the value of learning 'eta_s2':");
  scanf("%f",&eta_s2);
  printf("\nPlease input the value of learning 'alpha_s2':");
  scanf("%f",&alpha_s2);
  printf("\nPlease input the value of learning 'eta_s3':");
  scanf("%f",&eta_s3);
  printf("\nPlease input the value of learning 'alpha_s3':");
  scanf("%f",&alpha_s3);
  printf("\nPlease input the value of pass:");
  scanf("%d",&PASS);
}

/*------------ new desired set point by Reference Model ------------------*/
void rmodel(void)
{
  yr=4.0;
}

/* ----------------------- I/P --> PLANT --> O/P -----------------------------*/
void forward_plant(void)
{
     float a1,a2;
     float b1,b2,b3,b4,c1,c2,c3,c4;
     float aa1,aa2;
     float bb1,bb2,bb3,bb4,cc1,cc2,cc3,cc4;
     float y_dd;

     a1 = y_plant;
     a2 = ydot;
     c1 = a2;
     b1 = plant1(a1,c1);
     c2 = a2+th/2*b1;
     b2 = plant1(a1+th/2*c1,c2);
     c3 = a2+th/2*b2;
     b3 = plant1(a1+th/2*c2,c3);
     c4 = a2+th/2*b3;
     b4 = plant1(a1+th*c3,c4);

              /*----------- for calculating yu ------------*/
     aa1 = y_plant;
     aa2 = ydot;
     u_plant_dd=u_plant+dd;
     cc1 = aa2;
     bb1 = plant2(aa1,cc1);
     cc2 = aa2+th/2*bb1;
     bb2 = plant2(aa1+th/2*cc1,cc2);
     cc3 = aa2+th/2*bb2;
     bb3 = plant2(aa1+th/2*cc2,cc3);
     cc4 = aa2+th/2*bb3;
     y_plant_dd = y_plant+ th/6*(cc1+2*cc2+2*cc3+cc4);

     y_plant += th/6*(c1+2*c2+2*c3+c4);
     ydot += th/6*(b1+2*b2+2*b3+b4);

     yu=(y_plant_dd-y_plant)/dd;
}

float plant1(float ae,float af)
{
      float den2;

      den2 = 20.0*u_plant-20.0*ae-8.0*af;
      return(den2);
}

float plant2(float ae,float af)
{
      float den2;

      den2 = 20.0*u_plant_dd-20.0*ae-8.0*af;
      return(den2);
}

/* --------------- Controller Parameters Adjustment --------------------------*/
void backward_c(void)
{
  float deltau,deltay;
  float temp1, temp2;

  /* ----- delta4 adjustment ----*/
  delta4=(yr-y_plant)*yu;
  delta3=delta4*s3;              /* ----- delta3 adjustment ----*/

  delta21=delta3*py_pe;  /* ----- delta21 and delta22 adjustment --*/
  delta22=delta3*py_pce;

  ds3=eta_s3*delta4*out_t + alpha_s3*ds3;   /*--- s1,s2, and s3 adjustment ---*/
  s3+=ds3;

  if((es1==1)||(es1== -1))
     ds1=alpha_s1*ds1;
     else
       ds1=eta_s1*delta21*e+alpha_s1*ds1;
  if((ces2==1)||(ces2== -1))
     ds2=alpha_s2*ds2;
     else
       ds2=eta_s2*delta22*ce+alpha_s2*ds2;
  s1+=ds1;
  s2+=ds2;
}

void s1s2_adjust(void)
{
    if(s1<0.0)
       s1 = -s1;
    if(s2<0.0)
       s2 = -s2;

    es1=e*s1;
    ces2=ce*s2;

    if(es1>1.0)   /* e must be positive */
      {
        es1=1.0;
        s1=1.0/e;
      }
    if(es1< -1.0)   /* e must be negative */
      {
       es1= -1.0;
       s1= -1.0/e;
      }

    if(ces2>1.0)   /* ce must be positive */
      {
        ces2=1.0;
        s2=1.0/ce;
      }
    if(ces2< -1.0)   /* ce must be negative */
      {
       ces2= -1.0;
       s2= -1.0/ce;
      }
}

/* --------- Determine four corners according ces2 and es1 -----------------*/
/* -1 -0.8 -0.6 -0.4 -0.2   0 0.2 0.4 0.6 0.8   1 */
/*  0    1    2    3    4   5   6   7   8   9  10 */
void corner(void)
{
  float y,fq,x;
  int inty;

  y=5.0*es1+5.0;
  qe_left=(int)(y);
  qe_right=qe_left+1;
  fq=(float)(qe_left);
  e_left=(fq-5.0)/5.0;
  fq=(float)(qe_right);
  e_right=(fq-5.0)/5.0;

  y=5.0*ces2+5.0;
  qce_up=(int)(y);
  qce_down=qce_up+1;
  fq=(float)(qce_up);
  ce_up=(fq-5.0)/5.0;
  fq=(float)(qce_down);
  ce_down=(fq-5.0)/5.0;
}


float ctrl_o_decide(void)
{
 float A,B,C,D,temp;
 float per,iter1,iter2;

 A=decision_t[qce_up][qe_left];
 B=decision_t[qce_up][qe_right];
 C=decision_t[qce_down][qe_left];
 D=decision_t[qce_down][qe_right];

 if((qe_right==11)&&(qce_down==11))
     return(A);
 if(qe_right==11)
   {
     per=ces2-ce_up;
     temp=iterpolation(A,C,per);
     return(temp);
   }
 if(qce_down==11)
   {
     per=es1-e_left;
     temp=iterpolation(A,B,per);
     return(temp);
   }

 per=es1-e_left;
 iter1=iterpolation(A,B,per);
 iter2=iterpolation(C,D,per);
 per=ces2-ce_up;
 temp=iterpolation(iter1,iter2,per);
 return(temp);
}

float py_pe_decide(void)
{
 float A,B,C,D,temp;
 float per,iter1,iter2;

 A=py_pe_t[qce_up][qe_left];
 B=py_pe_t[qce_up][qe_right];
 C=py_pe_t[qce_down][qe_left];
 D=py_pe_t[qce_down][qe_right];

 if((qe_right==11)&&(qce_down==11))
     return(A);
 if(qe_right==11)
   {
     per=ces2-ce_up;
     temp=iterpolation(A,C,per);
     return(temp);
   }
 if(qce_down==11)
   {
     per=es1-e_left;
     temp=iterpolation(A,B,per);
     return(temp);
   }

 per=es1-e_left;
 iter1=iterpolation(A,B,per);
 iter2=iterpolation(C,D,per);
 per=ces2-ce_up;
 temp=iterpolation(iter1,iter2,per);
 return(temp);
}

float py_pce_decide(void)
{
 float A,B,C,D,temp;
 float per,iter1,iter2;

 A=py_pce_t[qce_up][qe_left];
 B=py_pce_t[qce_up][qe_right];
 C=py_pce_t[qce_down][qe_left];
 D=py_pce_t[qce_down][qe_right];

 if((qe_right==11)&&(qce_down==11))
     return(A);
 if(qe_right==11)
   {
     per=ces2-ce_up;
     temp=iterpolation(A,C,per);
     return(temp);
   }
 if(qce_down==11)
   {
     per=es1-e_left;
     temp=iterpolation(A,B,per);
     return(temp);
   }

 per=es1-e_left;
 iter1=iterpolation(A,B,per);
 iter2=iterpolation(C,D,per);
 per=ces2-ce_up;
 temp=iterpolation(iter1,iter2,per);
 return(temp);
}

float iterpolation(float a, float b, float p)
{
 float x;

 x=p*(b-a)/SPACE +a;
 return(x);
}

void op_file_def(void)
{
  strcpy(scalor,"scalor.m");
  strcpy(error,"error.m");
  strcpy(output,"output.m");
  strcpy(l_curve,"l_curve.m");
  strcpy(plant_u,"plant_u.m");
  strcpy(scalor_in,"sf_in.m");

  if((fscalor=fopen(scalor,"wt"))==NULL)
   {
    printf("\aERROR : Cannot open file \"%s\" ",scalor);
    exit(0);
   }
  if((ferror=fopen(error,"wt"))==NULL)
   {
    printf("\aERROR : Cannot open file \"%s\" ",error);
    exit(0);
   }
  if((foutput=fopen(output,"wt"))==NULL)
   {
    printf("\aERROR : Cannot open file \"%s\" ",output);
    exit(0);
   }
  if((fl_curve=fopen(l_curve,"wt"))==NULL)
   {
    printf("\aERROR : Cannot open file \"%s\" ",l_curve);
    exit(0);
   }
  if((fplant_u=fopen(plant_u,"wt"))==NULL)
   {
    printf("\aERROR : Cannot open file \"%s\" ",plant_u);
    exit(0);
   }
  if((fscalor_in=fopen(scalor_in,"wt"))==NULL)
   {
    printf("\aERROR : Cannot open file \"%s\" ",scalor_in);
    exit(0);
   }
}