#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;
	  
  }
}

/* Lookup table structure */
typedef struct {
  float valueLo;
  unsigned int iHi;
  float uSpacing;
} map_data;

/* Macro definitions */
#define TRUE 1
#define FALSE 0

/* Global variable declarations */
extern unsigned short inp_val[];
extern map_data angle_norm_map;
extern float angle_norm_vals[11];
extern map_data pos_norm_map;
extern float pos_norm_vals[11];
extern float inp_volt[2];
extern float integral_state;
extern float duty_cycle;
extern float direction;
extern float normalized_error;
extern unsigned char read_failure;

/* Function declarations */
void control_task(void);
void look_up_even( float *pY, float u, map_data map, float *pYData);
extern void read_inputs(void);
extern void set_outputs(void);
extern void get_control_gains(float* c_prop, float* c_int);