#include "qt_pi.h"

typedef struct {
  
  float TargetTicksPerFrame;
  long Encoder;
  long PrevEnc;

  int PrevInput;
  int ITerm;
  
  uint8_t Kp = 45;
  uint8_t Kd = 0;
  uint8_t Ki = 5;
  uint8_t Ko = 3;
  
  uint8_t base_pwm = 70;
  
  long output;

}

SetPointInfo;

SetPointInfo leftPID, rightPID;

bool isMoving = false;

void resetPID(){

  leftPID.TargetTicksPerFrame = 0.0;
  leftPID.Encoder = readEncoder(LEFT_WHEEL);
  leftPID.PrevEnc = leftPID.Encoder;
  leftPID.output = 0;
  leftPID.PrevInput = 0;
  leftPID.ITerm = 0;
  
  rightPID.TargetTicksPerFrame = 0.0;
  rightPID.Encoder = readEncoder(RIGHT_WHEEL);
  rightPID.PrevEnc = rightPID.Encoder;
  rightPID.output = 0;
  rightPID.PrevInput = 0;
  rightPID.ITerm = 0;

}

void doPID(SetPointInfo * p) {
  
  long Perror;
  long output;
  int input;
  
  input = p->Encoder - p->PrevEnc;
  Perror = p->TargetTicksPerFrame - input;
  
  p->ITerm += (p->Ki) * Perror;
  output = ((p->Kp) * Perror + (p->Kd) * (input - p->PrevInput) + p->ITerm) / (p->Ko);
  
  if(p->TargetTicksPerFrame > 0)
  {
    output += p->base_pwm;
  }
  else if(p->TargetTicksPerFrame < 0) 
  { 
    output -= p->base_pwm;
  }
  
  p->PrevEnc = p->Encoder;
  
  if (output >= MAX_PWM)
  {
    output = MAX_PWM;
  }
  else if (output <= -MAX_PWM)
  {
    output = -MAX_PWM;
  }
  else
  {
    p->output = output;
  }
  
  p->PrevInput = input;
}

void updatePID() {

  if (!isMoving){
    
    if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0)
      resetPID();
      
    return;
  }

  if(leftPID.TargetTicksPerFrame != 0)
  {
    leftPID.Encoder = readEncoder(LEFT_WHEEL);
    doPID(&leftPID);
  }
  
  if(rightPID.TargetTicksPerFrame != 0)
  {
    rightPID.Encoder = readEncoder(RIGHT_WHEEL);
    doPID(&rightPID);
  }
  
  if((abs(leftPID.output) < 70) && (abs(rightPID.output) < 70))
    setMotorSpeeds(0, 0);
  
  else if(abs(leftPID.output) < 70)
    setMotorSpeeds(0, rightPID.output);
    
  else if(abs(rightPID.output) < 70)
    setMotorSpeeds(leftPID.output, 0);

  else
    setMotorSpeeds(leftPID.output, rightPID.output);
}

void setPID_L(uint8_t *buf){
   
  leftPID.Kp = buf[0];
  leftPID.Kd = buf[1];
  leftPID.Ki = buf[2];
  leftPID.Ko = buf[3];

}

void setPID_R(uint8_t *buf){
                  
  rightPID.Kp = buf[0];
  rightPID.Kd = buf[1];
  rightPID.Ki = buf[2];
  rightPID.Ko = buf[3];
}

void moveTimeout(){  
  setMoving(false);
}

void setMoving(bool t){

  if((!t) && (isMoving))
  {
    setMotorSpeeds(0, 0);
    resetPID();
  }
  
  isMoving = t;
}

void getPID(uint8_t *buf){
  
  buf[0] = leftPID.Kp;
  buf[1] = leftPID.Kd;
  buf[2] = leftPID.Ki;
  buf[3] = leftPID.Ko;
  
  buf[4] = rightPID.Kp;
  buf[5] = rightPID.Kd;
  buf[6] = rightPID.Ki;
  buf[7] = rightPID.Ko;
  
}

void setTicksPerFrame(uint8_t *buf){

  int32_t l, r;
  
  memcpy(&l, buf, 4);
  memcpy(&r, buf + 4, 4);

  leftPID.TargetTicksPerFrame  = l / 1000.0;
  rightPID.TargetTicksPerFrame = r / 1000.0;

  if(l | r)
    setMoving(true);
  else
    setMoving(false);
}
