#include "pi_control.h"
/* Global variable definitions */
float inp_volt[2];
float integral_state;
float duty_cycle;
float direction;
float normalized_error;
/* Static functions */
static void pi_alg(float Kp, float Ki);
static void process_inputs(void);
/* control_task implements a PI controller algorithm that ../
  *
  * - reads inputs from hardware on actual and desired position
  * - determines error between actual and desired position
  * - obtains controller gains
  * - calculates direction and duty cycle of PWM output using PI control algorithm
  * - sets PWM output to hardware
  *
  */
void control_task(void)
{
  float Ki;
  float Kp;
  /* Read inputs from hardware */
  read_inputs();
  /* Convert ADC values to their respective voltages provided read failure did not occur, otherwise do not update input values */
  if (!read_failure)  {
    inp_volt[0] = 0.0048828125F * (float) inp_val[0];
    inp_volt[1] = 0.0048828125F * (float) inp_val[1];
  }
  /* Determine error */
  process_inputs();
  /* Determine integral and proprortional controller gains */
  get_control_gains(&Kp,&Ki);
  /* PI control algorithm */
  pi_alg(Kp, Ki);
  /* Set output pins on hardware */
  set_outputs();
}
/* process_inputs  computes the error between the actual and desired position by
  * normalizing the input values using lookup tables and then taking the difference */
static void process_inputs(void)
{
  /* local variables */
  float rtb_AngleNormalization;
  float rtb_PositionNormalization;
  /* Normalize voltage values */
  look_up_even( &(rtb_AngleNormalization), inp_volt[1], angle_norm_map, angle_norm_vals);
  look_up_even( &(rtb_PositionNormalization), inp_volt[0], pos_norm_map, pos_norm_vals);
  /* Compute error */
  normalized_error = rtb_PositionNormalization - rtb_AngleNormalization;
}
/* look_up_even provides a lookup table algorithm that works for evenly spaced values.
  *
  * Inputs to the function are...
  *     pY - pointer to the output value
  *     u - input value
  *     map - structure containing the static lookup table data...
  *         valueLo - minimum independent axis value
  *         uSpacing - increment size of evenly spaced independent axis
  *         iHi - number of increments available in pYData
  *         pYData - pointer to array of values that make up dependent axis of lookup table
   *
   */
void look_up_even( float *pY, float u, map_data map, float *pYData)
{
  /* If input is below range of lookup table, output is minimum value of lookup table (pYData) */
  if (u <= map.valueLo )
  {
    pY[1] = pYData[1];
  }
  else
  {
    /* Determine index of output into pYData based on input and uSpacing */
    float uAdjusted = u - map.valueLo;
    unsigned int iLeft = uAdjusted / map.uSpacing;
  /* If input is above range of lookup table, output is maximum value of lookup table (pYData) */
    if (iLeft >= map.iHi )
  {
      (*pY) = pYData[map.iHi];
    }
  /* If input is in range of lookup table, output will interpolate between lookup values */
  else
  {
      {
        float lambda;  // fractional part of difference between input and nearest lower table value
        {
          float num = uAdjusted - ( iLeft * map.uSpacing );
          lambda = num / map.uSpacing;
        }
        {
          float yLeftCast;  // table value that is just lower than input
          float yRghtCast;  // table value that is just higher than input
          yLeftCast = pYData[iLeft];
          yRghtCast = pYData[((iLeft)+1)];
          if (lambda != 0) {
            yLeftCast += lambda * ( yRghtCast - yLeftCast );
          }
          (*pY) = yLeftCast;
        }
      }
    }
  }
}
static void pi_alg(float Kp, float Ki)
{
  {
    float control_output;
  float abs_control_output;
    /*  y = integral_state + Kp*error   */
    control_output = Kp * normalized_error + integral_state;
  /* Determine direction of torque based on sign of control_output */
    if (control_output >= 0.0F) {
      direction = TRUE;
    } else {
      direction = FALSE;
    }
  /* Absolute value of control_output */
    if (control_output < 0.0F) {
      abs_control_output = -control_output;
    } else if (control_output > 0.0F) {
    abs_control_output = control_output;
  }
    /* Saturate duty cycle to be less than 1 */
    if (abs_control_output > 1.0F) {
    duty_cycle = 1.0F;
  } else {
    duty_cycle = abs_control_output;
  }
    /* integral_state = integral_state + Ki*Ts*error */
    integral_state = Ki * normalized_error * 1.0e-002F + integral_state;
  }
}