#include <control.h>

int main() {

  CControl plant,
           comp,
           fb_sys,
           *ol_sys,
           *cl_sys;
  CPlot rlocus_plot,
        step_plot;
  double plant_k,
         comp_k,
         tr,
         ts,
         os;
  array double fb_num[] = {1},
               fb_den[] = {1};
  array double complex dominant_p[1],
                       comp_p[1],
                       comp_z[1];

  int i, 
      noz,
      nop;

  printf("\nValues inside brackets are default values.\n"
         "Press Enter to adopt default values or type in different values.\n");
  printf("\nDefine the plant:\n");
  printf("  number of zeros [0]: ");
  noz = getnum(NULL, 0);

  array double complex plant_z[noz];
  double real_plant_z, 
         imag_plant_z;

  if(noz > 0) {
    for(i=0; i<noz; i++) {
      printf("    #%d zero:\n", i+1);
      printf("      real part: ");
      real_plant_z = getnum(NULL, 0);
      printf("      imaginary part: ");
      imag_plant_z = getnum(NULL, 0);
      plant_z[i] = complex(real_plant_z, imag_plant_z);
    }
  }

  printf("  number of poles [2]: ");
  nop = getnum(NULL, 2);

  array double complex plant_p[nop];
  double real_plant_p, 
         imag_plant_p;

  if(nop == 2) {
    for(i=0; i<nop; i++) {
      printf("    #%d pole:\n", i+1);
      if(i == 0) {
        printf("      real part [0]: ");
        real_plant_p = getnum(NULL, 0);
        printf("      imaginary part [0]: ");
        imag_plant_p = getnum(NULL, 0);
      }
      if(i == 1) {
        printf("      real part [-2]: ");
        real_plant_p = getnum(NULL, -2);
        printf("      imaginary part [0]: ");
        imag_plant_p = getnum(NULL, 0);
      }
      plant_p[i] = complex(real_plant_p, imag_plant_p);
    }
  }
  else if(nop > 0 && nop != 2) {
    for(i=0; i<nop; i++) {
      printf("    #%d pole:\n", i+1);
      printf("      real part: ");
      real_plant_p = getnum(NULL, 0);
      printf("      imaginary part: ");
      imag_plant_z = getnum(NULL, 0);
      plant_p[i] = complex(real_plant_p, imag_plant_p);
    }
  }

  printf("  gain [4]: ");
  plant_k = getnum(NULL, 4);

  double real_dominant_p,
         imag_dominant_p;
  printf("\nSpecify a pole for the dominant pole pair:\n");
  printf("  real part [-2]: ");
  real_dominant_p = getnum(NULL, -2);
  printf("  imaginary part [3.4641]: ");
  imag_dominant_p = getnum(NULL, 3.4641);
  dominant_p[0] = complex(real_dominant_p, imag_dominant_p);

  int comp_pz_flag;
  printf("\nSpecify the compensator (0: pole 1: zero) [0]: ");
  comp_pz_flag = getnum(NULL, 0);
  if(comp_pz_flag == 0) {
    printf("  pole [-20]: ");
    comp_p[0] = getnum(NULL, -20);
  }
  else if(comp_pz_flag == 1) {
    printf("  zero [-4]: ");
    comp_z[0] = getnum(NULL, -4);
  }
  
  plant.model("zpk", NULL, plant_p, plant_k);
  compensatorZeroPoleGain(&plant, dominant_p, comp_z, comp_p, &comp_k);
  comp.model("zpk", comp_z, comp_p, comp_k);
  fb_sys.model("tf", fb_num, fb_den);
  ol_sys = comp.series(&plant);
  cl_sys = ol_sys->feedback(&fb_sys);
  cl_sys->stepinfo(&tr, &ts, &os, NULL, NULL, NULL, NULL, NULL);

  printf("\n\nCompensator:\n"
         "    Pole: %f"
         "    Zero: %f"
         "    Gain: %f\n\n", real(comp_p), real(comp_z), comp_k);
  printf("Rise time: %f\n", tr);
  printf("Settling time: %f\n", ts);
  printf("Percent overshoot: %f\n\n", os);

  ol_sys->rlocus(&rlocus_plot, NULL, NULL);
  cl_sys->grid(1);
  cl_sys->step(&step_plot, NULL, NULL, NULL, ts*2);

  return 0;
}