#include <control.h>

int main() {
  // Define the plant
  double plant_k = 4;
  array double complex plant_p[] = {0, -2};

  // Specify a pole for the dominant pole pair
  array double complex dominant_p[] ={complex(-2, 2*sqrt(3))};
 
  // Specify the compensator's pole/zero
  array double complex comp_p[1], comp_z[1] = {-3};

  // Necessary variables
  CControl plant, comp, fb_sys, *ol_sys, *cl_sys;
  CPlot rlocus_plot, step_plot;
  double comp_k, tr, ts, os;
  array double fb_num[] = {1}, fb_den[] = {1};
                        
  // Calculate compensator's pole/zero and gain as well as   
  // corresponding step response characteristics of the compensated system
  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);
 
  // Display compensator's information
  printf("\nCompensator:\n"
         "    Pole: %f"
         "    Zero: %f"
         "    Gain: %f\n\n", real(comp_p), real(comp_z), comp_k);
                                                                   
  // Display corresponding step response characteristics 
  printf("Rise time: %f\n", tr);
  printf("Settling time: %f\n", ts);
  printf("Percent overshoot: %f\n\n", os);
                                                                               
  // Display plots of root locus and step response for the compensated system
  ol_sys->rlocus(&rlocus_plot, NULL, NULL);
  cl_sys->grid(1);
  cl_sys->step(&step_plot, NULL, NULL, NULL, ts*2);

  return 0;
}