

/*
 * Include Files
 *
 */
#if defined(MATLAB_MEX_FILE)
#include "tmwtypes.h"
#include "simstruc_types.h"
#else
#include "rtwtypes.h"
#endif

/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */
#include <math.h>
#include <stdio.h>
/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */
#define u_width 1
#define y_width 1
/*
 * Create external references here.  
 *
 */
/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */
/* extern double func(double a); */
/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */

/*
 * Output functions
 *
 */
void actuateFlexinol_Outputs_wrapper(const int16_T *contractionDegree,
			const real_T *vInput,
			const real_T *vOut,
			const boolean_T *trainingMode,
			int16_T *pwmFreq,
			uint8_T *SerialPrint,
			const real_T *xD)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
/* This sample sets the output equal to the input
      y0[0] = u0[0]; 
 For complex signals use: y0[0].re = u0[0].re; 
      y0[0].im = u0[0].im;
      y1[0].re = u1[0].re;
      y1[0].im = u1[0].im;
*/

const float r1 = 2.7;   // Value of R1 in the Flexinol voltage divider
const int max_rF = 13;
const int min_rF = 7 ;


/*float vIn;              // The measured voltage going into the Flexinol voltage divider
float vOut;             // The measured voltage between R1 and the Flexinol*/
float rFlexinol;        // The calculated resistance of the Flexinol

char dac;//int dac;               // The byte value to write to the DAC


float avgActual;        // Used in testing to track the average resistance readings
float delta;            // Used in testing to track the difference between the target and actual values

int index = 0;                    // Used to reference array variables
float resistanceValues[11];   // Target resistance values !!!!!!
int dacValuesWarming[11];     // Calibrated DAC values when Flexinol is warming  
int dacValuesCooling[11];     // Calibrated DAC values when Flexinol is cooling




/*
 * byteWrite takes the value in dac and outputs it as pwm frequencie on analog pin
 */
void byteWrite(){
  	int val_pwm = 255 * (dac / 5);  // val_pwm:255 = volt:5
	*pwmFreq = val_pwm;
    
}

/*TODO-----
 * calibrate(){}
 *
 */



/**************************************************************************************************
  calcResist takes ADC readings at A0 and A1, converts them to voltages and uses the results to
  calculate the resistance of the Flexinol - the constant value of 0.004883 used here is equal to
  the reference voltage of 5 divided by the ADC resolution of 1024
**************************************************************************************************/
void calcResist(){
 /* vIn = analogRead (A0);  // take a throw-away reading to reset the ADC after switching pins
  delay (10);
  vIn = analogRead (A0);*/
  float vIn_tmp = (*vInput) * 0.004883;
  
  /*vOut = analogRead (A1);
  delay (10);
  
  vOut = analogRead (A1);*/
  float vOut_tmp = (*vOut) * 0.004883;
  
  rFlexinol = (r1 * vOut_tmp) / (vIn_tmp - vOut_tmp);
}
void printStringSerial(char* s){
    int i = 0;
    while (s[i] != '\n'){
        SerialPrint[0] = s[i++];
    }

}
void actuate(int  contractionDegree){
    
     index = contractionDegree;
     int outerLoop = 0;
     int innerLoop = 0;
//     Serial.print = (std::" ");
//     Serial.print ("Testing "); 
//     Serial.print (index * 10);
//     Serial.println("%");


    dac = dacValuesWarming[index];
    for (outerLoop = 0; outerLoop <= 4; outerLoop ++)
    {
      delta = 0;
      avgActual = 0;
      for (innerLoop = 0; innerLoop <= 49; innerLoop ++)
      {
        byteWrite();
        //delay (200);
        calcResist();
        avgActual = avgActual + rFlexinol;
        
        if (rFlexinol < resistanceValues[index])
        {
          dac = dacValuesCooling[index];   
        } 
        else {
          dac = dacValuesWarming[index];   
        }
        delta = delta + abs(resistanceValues[index] - rFlexinol);
      } 

      delta = delta /50;
      avgActual = avgActual /50;

      /* TO SEND BACK FOR CALC
      Serial.print ("Target: ");   
      Serial.print (resistanceValues[index], DEC);    
      Serial.print ("   Average Actual: ");
      Serial.print (avgActual, DEC);
      Serial.print ("   Delta: ");
      Serial.print (avgActual - resistanceValues[index], DEC);
      Serial.print ("   Average Deviation: ");
      Serial.println (delta, DEC);*/
    }  
  
}


   // calibrate();
    actuate(contractionDegree[0]);
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
}

/*
  * Updates function
  *
  */
void actuateFlexinol_Update_wrapper(const int16_T *contractionDegree,
			const real_T *vInput,
			const real_T *vOut,
			const boolean_T *trainingMode,
			int16_T *pwmFreq,
			uint8_T *SerialPrint,
			real_T *xD)
{
  /* %%%-SFUNWIZ_wrapper_Update_Changes_BEGIN --- EDIT HERE TO _END */
/*
 * Code example
 *   xD[0] = u0[0];
*/
/* %%%-SFUNWIZ_wrapper_Update_Changes_END --- EDIT HERE TO _BEGIN */
}
