#include <Arduino.h>
#include <ArduinoJson.h>
#include <string>
#include <Wire.h>
#include <INA219_WE.h>
#include "SPI.h"

#define RXpin 0 // Define your RX pin here
#define TXpin 0 // Define your TX pin here

bool debug = true;

//TO IMPLEMENT
//DONE 2 way serial
//DONE F<>,B<>,S,L<>,R<>,p<0--1023>
//DONE Obtain current and power usage, get voltage from analog pin
//request angle facing
//DONE speed control 0-1
//speed calibration, 0 stop and max speed to match
//distance travveled and x and y at request


//-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------//
INA219_WE ina219; // this is the instantiation of the library for the current sensor

float open_loop, closed_loop; // Duty Cycles
float vpd, vb, vref, iL, dutyref, current_mA; // Measurement Variables
unsigned int sensorValue0, sensorValue1, sensorValue2, sensorValue3; // ADC sample values declaration
float ev = 0, cv = 0, ei = 0, oc = 0; //internal signals
float Ts = 0.0008; //1.25 kHz control frequency. It's better to design the control period as integral multiple of switching period.
float kpv = 0.05024, kiv = 15.78, kdv = 0; // voltage pid.
float u0v, u1v, delta_uv, e0v, e1v, e2v; // u->output; e->error; 0->this time; 1->last time; 2->last last time
float kpi = 0.02512, kii = 39.4, kdi = 0; // current pid.
float u0i, u1i, delta_ui, e0i, e1i, e2i; // Internal values for the current controller
float uv_max = 4, uv_min = 0; //anti-windup limitation
float ui_max = 1, ui_min = 0; //anti-windup limitation
float current_limit = 1.0;
boolean Boost_mode = 0;
boolean CL_mode = 0;


unsigned int loopTrigger;
unsigned int com_count = 0; // a variables to count the interrupts. Used for program debugging.


//************************** Motor Constants **************************//
unsigned long previousMillis = 0; //initializing time counter
const long f_i = 10000;           //time to move in forward direction, please calculate the precision and conversion factor
const long r_i = 20000;           //time to rotate clockwise
const long b_i = 30000;           //time to move backwards
const long l_i = 40000;           //time to move anticlockwise
const long s_i = 50000;
int DIRRstate = LOW;              //initializing direction states
int DIRLstate = HIGH;

int DIRL = 20;                    //defining left direction pin
int DIRR = 21;                    //defining right direction pin

int pwmr = 5;                     //pin to control right wheel speed using pwm
int pwml = 9;                     //pin to control left wheel speed using pwm
//*******************************************************************//
//-------------------------------------------------------SMPS & MOTOR CODE END------------------------------------------------------//

//-------------------------------------------------------OPTICAL SENSOR CODE START------------------------------------------------------//
#define PIN_SS        10
#define PIN_MISO      12
#define PIN_MOSI      11
#define PIN_SCK       13



#define PIN_MOUSECAM_RESET     8
#define PIN_MOUSECAM_CS        7

#define ADNS3080_PIXELS_X                 30
#define ADNS3080_PIXELS_Y                 30

#define ADNS3080_PRODUCT_ID            0x00
#define ADNS3080_REVISION_ID           0x01
#define ADNS3080_MOTION                0x02
#define ADNS3080_DELTA_X               0x03
#define ADNS3080_DELTA_Y               0x04
#define ADNS3080_SQUAL                 0x05
#define ADNS3080_PIXEL_SUM             0x06
#define ADNS3080_MAXIMUM_PIXEL         0x07
#define ADNS3080_CONFIGURATION_BITS    0x0a
#define ADNS3080_EXTENDED_CONFIG       0x0b
#define ADNS3080_DATA_OUT_LOWER        0x0c
#define ADNS3080_DATA_OUT_UPPER        0x0d
#define ADNS3080_SHUTTER_LOWER         0x0e
#define ADNS3080_SHUTTER_UPPER         0x0f
#define ADNS3080_FRAME_PERIOD_LOWER    0x10
#define ADNS3080_FRAME_PERIOD_UPPER    0x11
#define ADNS3080_MOTION_CLEAR          0x12
#define ADNS3080_FRAME_CAPTURE         0x13
#define ADNS3080_SROM_ENABLE           0x14
#define ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER      0x19
#define ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER      0x1a
#define ADNS3080_FRAME_PERIOD_MIN_BOUND_LOWER      0x1b
#define ADNS3080_FRAME_PERIOD_MIN_BOUND_UPPER      0x1c
#define ADNS3080_SHUTTER_MAX_BOUND_LOWER           0x1e
#define ADNS3080_SHUTTER_MAX_BOUND_UPPER           0x1e
#define ADNS3080_SROM_ID               0x1f
#define ADNS3080_OBSERVATION           0x3d
#define ADNS3080_INVERSE_PRODUCT_ID    0x3f
#define ADNS3080_PIXEL_BURST           0x40
#define ADNS3080_MOTION_BURST          0x50
#define ADNS3080_SROM_LOAD             0x60

#define ADNS3080_PRODUCT_ID_VAL        0x17




int total_x = 0;

int total_y = 0;


int total_x1 = 0;

int total_y1 = 0;


int x=0;
int y=0;

int a=0;
int b=0;




int distance_x=0;
int distance_y=0;


  
volatile byte movementflag=0;
volatile int xydat[2];



int convTwosComp(int b){
  //Convert from 2's complement
  if(b & 0x80){
    b = -1 * ((b ^ 0xff) + 1);
    }
  return b;
  }



  int tdistance = 0;


void mousecam_reset()
{
  digitalWrite(PIN_MOUSECAM_RESET,HIGH);
  delay(1); // reset pulse >10us
  digitalWrite(PIN_MOUSECAM_RESET,LOW);
  delay(35); // 35ms from reset to functional
}


int mousecam_init()
{
  pinMode(PIN_MOUSECAM_RESET,OUTPUT);
  pinMode(PIN_MOUSECAM_CS,OUTPUT);
  
  digitalWrite(PIN_MOUSECAM_CS,HIGH);
  
  mousecam_reset();
  
  int pid = mousecam_read_reg(ADNS3080_PRODUCT_ID);
  if(pid != ADNS3080_PRODUCT_ID_VAL)
    return -1;

  // turn on sensitive mode
  mousecam_write_reg(ADNS3080_CONFIGURATION_BITS, 0x19);

  return 0;
}

void mousecam_write_reg(int reg, int val)
{
  digitalWrite(PIN_MOUSECAM_CS, LOW);
  SPI.transfer(reg | 0x80);
  SPI.transfer(val);
  digitalWrite(PIN_MOUSECAM_CS,HIGH);
  delayMicroseconds(50);
}

int mousecam_read_reg(int reg)
{
  digitalWrite(PIN_MOUSECAM_CS, LOW);
  SPI.transfer(reg);
  delayMicroseconds(75);
  int ret = SPI.transfer(0xff);
  digitalWrite(PIN_MOUSECAM_CS,HIGH); 
  delayMicroseconds(1);
  return ret;
}

struct MD
{
 byte motion;
 char dx, dy;
 byte squal;
 word shutter;
 byte max_pix;
};


void mousecam_read_motion(struct MD *p)
{
  digitalWrite(PIN_MOUSECAM_CS, LOW);
  SPI.transfer(ADNS3080_MOTION_BURST);
  delayMicroseconds(75);
  p->motion =  SPI.transfer(0xff);
  p->dx =  SPI.transfer(0xff);
  p->dy =  SPI.transfer(0xff);
  p->squal =  SPI.transfer(0xff);
  p->shutter =  SPI.transfer(0xff)<<8;
  p->shutter |=  SPI.transfer(0xff);
  p->max_pix =  SPI.transfer(0xff);
  digitalWrite(PIN_MOUSECAM_CS,HIGH); 
  delayMicroseconds(5);
}

// pdata must point to an array of size ADNS3080_PIXELS_X x ADNS3080_PIXELS_Y
// you must call mousecam_reset() after this if you want to go back to normal operation
int mousecam_frame_capture(byte *pdata)
{
  mousecam_write_reg(ADNS3080_FRAME_CAPTURE,0x83);
  
  digitalWrite(PIN_MOUSECAM_CS, LOW);
  
  SPI.transfer(ADNS3080_PIXEL_BURST);
  delayMicroseconds(50);
  
  int pix;
  byte started = 0;
  int count;
  int timeout = 0;
  int ret = 0;
  for(count = 0; count < ADNS3080_PIXELS_X * ADNS3080_PIXELS_Y; )
  {
    pix = SPI.transfer(0xff);
    delayMicroseconds(10);
    if(started==0)
    {
      if(pix&0x40)
        started = 1;
      else
      {
        timeout++;
        if(timeout==100)
        {
          ret = -1;
          break;
        }
      }
    }
    if(started==1)
    {
      pdata[count++] = (pix & 0x3f)<<2; // scale to normal grayscale byte range
    }
  }

  digitalWrite(PIN_MOUSECAM_CS,HIGH); 
  delayMicroseconds(14);
  
  return ret;
}

//-------------------------------------------------------OPTICAL SENSOR CODE END------------------------------------------------------//

//Tracker Variables
int current_x = 0;
int current_y = 0;
int goal_x = 0;
int goal_y = 0;
int distanceGoal;
bool commandComplete = 1;
float powerUsage_mWh = 0;
int distTravelled_mm = 0;
bool initialAngleSet=false;

//calibration varibles
int angularDrift = 0; //+ve to right, -ve to left
int leftStart = 80; //pwm min for left motor
int leftStop = 255; //pwm max for left motor
int rightStart = 80; //pwm min for right motor
int rightStop = 255; //pwm max for right motor


//Energy Usage Variables
unsigned long previousMillis_Energy = 0;        // will store last time energy use was updated
const long interval_Energy = 1000;         //energy usaged update frequency
float totalEnergyUsed = 0;
float powerUsed = 0;
int loopCount = 0;
float motorVoltage = 0;

int getPWMfromSpeed(float speedr,bool left) {
  if (speedr >= 1) {
    return 512;
  }
  else if (speedr < 0) {
    return 0;
  }
  else {
    int speedpercentage = (speedr * 100);
    if(left){
        return map(speedpercentage,0,100,leftStart,leftStop);
    }
    else{
        return map(speedpercentage,0,100,rightStart,rightStop);
    }
  }
}

void setup()
{
  //-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------//
  //************************** Motor Pins Defining **************************//
  pinMode(DIRR, OUTPUT);
  pinMode(DIRL, OUTPUT);
  pinMode(pwmr, OUTPUT);
  pinMode(pwml, OUTPUT);
  digitalWrite(pwmr, HIGH);       //setting right motor speed at maximum
  digitalWrite(pwml, HIGH);       //setting left motor speed at maximum
  //*******************************************************************//

  //Basic pin setups

  noInterrupts(); //disable all interrupts
  pinMode(13, OUTPUT);  //Pin13 is used to time the loops of the controller
  pinMode(3, INPUT_PULLUP); //Pin3 is the input from the Buck/Boost switch
  pinMode(2, INPUT_PULLUP); // Pin 2 is the input from the CL/OL switch
  analogReference(EXTERNAL); // We are using an external analogue reference for the ADC

  // TimerA0 initialization for control-loop interrupt.

  TCA0.SINGLE.PER = 999; //
  TCA0.SINGLE.CMP1 = 999; //
  TCA0.SINGLE.CTRLA = TCA_SINGLE_CLKSEL_DIV16_gc | TCA_SINGLE_ENABLE_bm; //16 prescaler, 1M.
  TCA0.SINGLE.INTCTRL = TCA_SINGLE_CMP1_bm;

  // TimerB0 initialization for PWM output

  pinMode(6, OUTPUT);
  TCB0.CTRLA = TCB_CLKSEL_CLKDIV1_gc | TCB_ENABLE_bm; //62.5kHz
  analogWrite(6, 120);

  interrupts();  //enable interrupts.
  Wire.begin(); // We need this for the i2c comms for the current sensor
  ina219.init(); // this initiates the current sensor
  Wire.setClock(700000); // set the comms speed for i2c
  //-------------------------------------------------------SMPS & MOTOR CODE END------------------------------------------------------//
  Serial.begin(115200);  // Set up hardware UART0 (Connected to USB port)
  Serial1.begin(9600); // Set up hardware UART1

  //Serial.println(getPWMfromSpeed(-1));
  //Serial.println(getPWMfromSpeed(256));
  //Serial.println(getPWMfromSpeed(0.5));
  // Other Drive setup stuff
  /////////currentHeading = REQUEST HEADING HERE;

          analogWrite(pwmr,0);
          analogWrite(pwml,0);
          //digitalWrite(DIRR, LOW);
          //digitalWrite(DIRL, HIGH);



          pinMode(PIN_SS,OUTPUT);
  pinMode(PIN_MISO,INPUT);
  pinMode(PIN_MOSI,OUTPUT);
  pinMode(PIN_SCK,OUTPUT);
  
  SPI.begin();
  SPI.setClockDivider(SPI_CLOCK_DIV32);
  SPI.setDataMode(SPI_MODE3);
  SPI.setBitOrder(MSBFIRST);
  
  Serial.begin(9600);

  if(mousecam_init()==-1)
  {
    Serial.println("Mouse cam failed to init");
    while(1);
  }

  
}

bool commandCompletionStatus = 1; //0-No Command, 1-New Command, 2-Command being run, 3-Command Complete
int requiredHeading = 0;
    int distance = 0;
    float spd = 0;
    int currentHeading = 0;
    //reset variables for update on completion
    
unsigned long previousMillis_Command = 0;
const long interval_Command = 1000; 
DeserializationError error;

char asciiart(int k)
{
  static char foo[] = "WX86*3I>!;~:,`. ";
  return foo[k>>4];
}

byte frame[ADNS3080_PIXELS_X * ADNS3080_PIXELS_Y];


void loop()
{

          analogWrite(pwmr,512);
          analogWrite(pwml,512);
          digitalWrite(DIRR, HIGH);
          digitalWrite(DIRL, LOW);
          Serial.println(pwmr);
          
  //Serial.println("Here");
  DynamicJsonDocument rdoc(1024); // receive doc, not sure how big this needs to be
  
  //deserializeJson(rdoc, Serial1); // Take JSON input from UART1

  //Check for updates every 1s...
  unsigned long currentMillis_Command = millis();

  if (currentMillis_Command - previousMillis_Command >= interval_Command) {
    // save the last time you blinked the LED
    previousMillis_Command = currentMillis_Command;
    error = deserializeJson(rdoc, Serial1);
    //Serial.println("H");
  }
  

  // Test if parsing succeeds.
  if (error) {
    //Serial.print(F("deserializeJson() failed: "));
    //Serial.println(error.f_str());
    return;
  }
  else{
    //parsing success, prepare command and pull request information
    commandCompletionStatus = 1;
     requiredHeading = rdoc["rH"];
     distance = rdoc["dist"];
     spd = rdoc["sp"];
     currentHeading = rdoc["cH"];
    //reset variables for update on completion
     commandComplete = 0;
     powerUsage_mWh = 0.0;
     distTravelled_mm = 0;
  }
 
//if(current_x!=goal_x)

  // Do Drive stuff, set the 5 values above

  if(commandCompletionStatus == 0){  //noCommand
   //Do Nothing just wait
  }
  if(commandCompletionStatus == 1){  //newCommand
    //set goals
    goal_x += distance*sin(requiredHeading);
    goal_y += distance*cos(requiredHeading);
    total_y = 0;
    total_x = 0;
    commandCompletionStatus = 2;

    initialAngleSet=false;
  }
  if(commandCompletionStatus == 2){  //ongoingCommand
    //start moving towards goal

    //set angle first
    if(!initialAngleSet){
      //turn to angle
      currentHeading = getCurrentHeading();
      if(currentHeading<requiredHeading){ //turn right
        analogWrite(pwmr,getPWMfromSpeed(spd,false));
          analogWrite(pwml,getPWMfromSpeed(spd,true));
          digitalWrite(DIRR, LOW);
          digitalWrite(DIRL, LOW);
      }
      else if(currentHeading>requiredHeading){ //turn left
        analogWrite(pwmr,getPWMfromSpeed(spd,false));
          analogWrite(pwml,getPWMfromSpeed(spd,true));
          digitalWrite(DIRR, HIGH);
          digitalWrite(DIRL, HIGH);
      }
      else{
        //heading correct therefore move to next step...
        //STOP!!!!
        digitalWrite(pwmr,LOW);
        digitalWrite(pwml,LOW);
        
        initialAngleSet = true;
      }
    }
    else{    //then move forwards but check angle for drift
      if(total_x - distance > 0){ //go forwards
          analogWrite(pwmr,getPWMfromSpeed(spd,false));
          analogWrite(pwml,getPWMfromSpeed(spd,true));
          digitalWrite(DIRR, HIGH);
          digitalWrite(DIRL, LOW);
      }
      else if(total_x - distance < 0){//go backwards
          analogWrite(pwmr,getPWMfromSpeed(spd,false));
          analogWrite(pwml,getPWMfromSpeed(spd,true));
          digitalWrite(DIRR, LOW);
          digitalWrite(DIRL, HIGH);
      }
    else if((total_x == distance)) {//distance met
        //STOP!!!!!
        digitalWrite(pwmr,LOW);
        digitalWrite(pwml,LOW);
        commandCompletionStatus = 3;
        initialAngleSet = true;
      }
    }

  }
  if(commandCompletionStatus == 3){  //currentPosMatchesOrExceedsRequest
   ///finish moving
    
    //send update via UART

    //prepare feedback variables
    commandComplete = true;
  current_x = goal_x;
  current_y = goal_y;
  distTravelled_mm += distance;
       //compile energy use
       unsigned long currentMillis_Energy = millis();

    totalEnergyUsed += (currentMillis_Energy - previousMillis_Energy) * (powerUsed / loopCount) / 1000 / (60 * 60);
    previousMillis_Energy = currentMillis_Energy;
    if (debug) {
      Serial.print(motorVoltage);
      Serial.print("Energy Used: ");
      Serial.print(totalEnergyUsed);
      Serial.println("mWh");
    }

    loopCount = 0; //reset counter to zero
    powerUsed = 0;//reset power usage
    powerUsage_mWh = totalEnergyUsed;
    totalEnergyUsed = 0;
    
    DynamicJsonDocument tdoc(1024); // transmit doc, not sure how big this needs to be
    tdoc["comp"] = commandComplete;
    tdoc["mWh"] = powerUsage_mWh;
    tdoc["mm"] = distTravelled_mm;
    tdoc["pos"][0] = current_x;
    tdoc["pos"][1] = current_y;
    serializeJson(tdoc, Serial1); // Build JSON and send on UART1
  commandCompletionStatus = 0;
    
  }

  //Handle power usage
  //find motor voltage
  //int motorVSensor = analogRead(A5);
  //float motorVoltage = motorVSensor * (5.0 / 1023.0);
  float motorVoltage = 5;

  //find average power

  if(current_mA >=0){
    powerUsed += current_mA * motorVoltage;
  }
  Serial.println(powerUsed);

  //calculate averages for energy use calculations
  loopCount += 1; //handle loop quantity for averaging

  //find average current
  //find average voltage

  //update command/control


  if(movementflag){
 
    tdistance = tdistance + convTwosComp(xydat[0]);
    Serial.println("Distance = " + String(tdistance));
    movementflag=0;
    delay(3);
    }
    int val = mousecam_read_reg(ADNS3080_PIXEL_SUM);
  MD md;
  mousecam_read_motion(&md);
  for(int i=0; i<md.squal/4; i++)
    Serial.print('*');
  Serial.print(' ');
  Serial.print((val*100)/351);
  Serial.print(' ');
  Serial.print(md.shutter); Serial.print(" (");
  Serial.print((int)md.dx); Serial.print(',');
  Serial.print((int)md.dy); Serial.println(')');

  // Serial.println(md.max_pix);
  delay(100);


    distance_x = md.dx; //convTwosComp(md.dx);
    distance_y = md.dy; //convTwosComp(md.dy);

total_x1 = (total_x1 + distance_x);
total_y1 = (total_y1 + distance_y);

total_x = 10*total_x1/157; //Conversion from counts per inch to mm (400 counts per inch)
total_y = 10*total_y1/157; //Conversion from counts per inch to mm (400 counts per inch)
    

 Serial.print('\n');


Serial.println("Distance_x = " + String(total_x));

Serial.println("Distance_y = " + String(total_y));
 Serial.print('\n');
  //-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------//
  unsigned long currentMillis = millis();
  if (loopTrigger) { // This loop is triggered, it wont run unless there is an interrupt

    digitalWrite(13, HIGH);   // set pin 13. Pin13 shows the time consumed by each control cycle. It's used for debugging.

    // Sample all of the measurements and check which control mode we are in
    sampling();
    CL_mode = digitalRead(3); // input from the OL_CL switch
    Boost_mode = digitalRead(2); // input from the Buck_Boost switch

    if (Boost_mode) {
      if (CL_mode) { //Closed Loop Boost
        pwm_modulate(1); // This disables the Boost as we are not using this mode
      } else { // Open Loop Boost
        pwm_modulate(1); // This disables the Boost as we are not using this mode
      }
    } else {
      if (CL_mode) { // Closed Loop Buck
        // The closed loop path has a voltage controller cascaded with a current controller. The voltage controller
        // creates a current demand based upon the voltage error. This demand is saturated to give current limiting.
        // The current loop then gives a duty cycle demand based upon the error between demanded current and measured
        // current
        current_limit = 3; // Buck has a higher current limit
        ev = vref - vb;  //voltage error at this time
        cv = pidv(ev); //voltage pid
        cv = saturation(cv, current_limit, 0); //current demand saturation
        ei = cv - iL; //current error
        closed_loop = pidi(ei); //current pid
        closed_loop = saturation(closed_loop, 0.99, 0.01); //duty_cycle saturation
        pwm_modulate(closed_loop); //pwm modulation
      } else { // Open Loop Buck
        current_limit = 3; // Buck has a higher current limit
        oc = iL - current_limit; // Calculate the difference between current measurement and current limit
        if ( oc > 0) {
          open_loop = open_loop - 0.001; // We are above the current limit so less duty cycle
        } else {
          open_loop = open_loop + 0.001; // We are below the current limit so more duty cycle
        }
        open_loop = saturation(open_loop, dutyref, 0.02); // saturate the duty cycle at the reference or a min of 0.01
        pwm_modulate(open_loop); // and send it out
      }
    }
    // closed loop control path

    digitalWrite(13, LOW);   // reset pin13.
    loopTrigger = 0;
  }

  //************************** Motor Testing **************************//
  //this part of the code decides the direction of motor rotations depending on the time lapsed. currentMillis records the time lapsed once it is called.


  //*******************************************************************//
  //-------------------------------------------------------SMPS & MOTOR CODE END------------------------------------------------------//

}

int getCurrentHeading(){
  int currentAngle = 0;  //-------------___<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
  return currentAngle;
}

// Timer A CMP1 interrupt. Every 800us the program enters this interrupt.
// This, clears the incoming interrupt flag and triggers the main loop.

ISR(TCA0_CMP1_vect) {
  TCA0.SINGLE.INTFLAGS |= TCA_SINGLE_CMP1_bm; //clear interrupt flag
  loopTrigger = 1;
}

// This subroutine processes all of the analogue samples, creating the required values for the main loop

void sampling() {

  // Make the initial sampling operations for the circuit measurements

  sensorValue0 = analogRead(A0); //sample Vb
  sensorValue2 = analogRead(A2); //sample Vref
  sensorValue3 = analogRead(A3); //sample Vpd
  current_mA = ina219.getCurrent_mA(); // sample the inductor current (via the sensor chip)

  // Process the values so they are a bit more usable/readable
  // The analogRead process gives a value between 0 and 1023
  // representing a voltage between 0 and the analogue reference which is 4.096V

  vb = sensorValue0 * (4.096 / 1023.0); // Convert the Vb sensor reading to volts
  vref = sensorValue2 * (4.096 / 1023.0); // Convert the Vref sensor reading to volts
  vpd = sensorValue3 * (4.096 / 1023.0); // Convert the Vpd sensor reading to volts

  // The inductor current is in mA from the sensor so we need to convert to amps.
  // We want to treat it as an input current in the Boost, so its also inverted
  // For open loop control the duty cycle reference is calculated from the sensor
  // differently from the Vref, this time scaled between zero and 1.
  // The boost duty cycle needs to be saturated with a 0.33 minimum to prevent high output voltages

  if (Boost_mode == 1) {
    iL = -current_mA / 1000.0;
    dutyref = saturation(sensorValue2 * (1.0 / 1023.0), 0.99, 0.33);
  } else {
    iL = current_mA / 1000.0;
    dutyref = sensorValue2 * (1.0 / 1023.0);
  }

}

float saturation( float sat_input, float uplim, float lowlim) { // Saturatio function
  if (sat_input > uplim) sat_input = uplim;
  else if (sat_input < lowlim ) sat_input = lowlim;
  else;
  return sat_input;
}

void pwm_modulate(float pwm_input) { // PWM function
  analogWrite(6, (int)(255 - pwm_input * 255));
}

// This is a PID controller for the voltage

float pidv( float pid_input) {
  float e_integration;
  e0v = pid_input;
  e_integration = e0v;

  //anti-windup, if last-time pid output reaches the limitation, this time there won't be any intergrations.
  if (u1v >= uv_max) {
    e_integration = 0;
  } else if (u1v <= uv_min) {
    e_integration = 0;
  }

  delta_uv = kpv * (e0v - e1v) + kiv * Ts * e_integration + kdv / Ts * (e0v - 2 * e1v + e2v); //incremental PID programming avoids integrations.there is another PID program called positional PID.
  u0v = u1v + delta_uv;  //this time's control output

  //output limitation
  saturation(u0v, uv_max, uv_min);

  u1v = u0v; //update last time's control output
  e2v = e1v; //update last last time's error
  e1v = e0v; // update last time's error
  return u0v;
}

// This is a PID controller for the current

float pidi(float pid_input) {
  float e_integration;
  e0i = pid_input;
  e_integration = e0i;

  //anti-windup
  if (u1i >= ui_max) {
    e_integration = 0;
  } else if (u1i <= ui_min) {
    e_integration = 0;
  }

  delta_ui = kpi * (e0i - e1i) + kii * Ts * e_integration + kdi / Ts * (e0i - 2 * e1i + e2i); //incremental PID programming avoids integrations.
  u0i = u1i + delta_ui;  //this time's control output

  //output limitation
  saturation(u0i, ui_max, ui_min);

  u1i = u0i; //update last time's control output
  e2i = e1i; //update last last time's error
  e1i = e0i; // update last time's error
  return u0i;
}