#ifndef DEBUG_H
#define DEBUG_H

#include "LedPanel.h"
#include "Constants.h"

LedPanel panel(NUMPIXELS, PIN_PIXELS);

// Function Declarations
void debugPIDConstants(int lednum, double oldval, double newval);

void Debug_Setup(){
  panel.beginPanel();
}

void Debug_LoopStart(){
  panel.singleLED(DEBUG_BASESPEED, BASE_SPEED, BASE_SPEED, BASE_SPEED);
}

void Debug_PauseState(){
  panel.singleLED(DEBUG_STATE, 10, 10, 0);
}

void Debug_StandbyState(){
  panel.singleLED(DEBUG_STATE, 10, 0, 0);
}

void Debug_GoalFound(){
  panel.singleLED(DEBUG_STATE, 0, 10, 0);
}

void Debug_BallDetected(){
  panel.singleLED(DEBUG_STATE, 0, 10, 0);
}

void Debug_GoalSeeking(){
  // panel.resetPanel();
  panel.singleLED(DEBUG_STATE, 10, 10, 10);
}

void Debug_SetPIDConstants(double &p_constant, double &i_constant, double &d_constant, double new_p, double new_i, double new_d){
  unsigned long t = millis();
  debugPIDConstants(DEBUG_KP, p_constant, new_p);
  debugPIDConstants(DEBUG_KI, i_constant, new_i);
  debugPIDConstants(DEBUG_KD, d_constant, new_d);

  p_constant = new_p;
  i_constant = new_i;
  d_constant = new_d;
  
  while(millis() < t+2000); //debugging LED will light up for 2 seconds
  
  panel.singleLED(DEBUG_KP, 0, 0, 0);
  panel.singleLED(DEBUG_KI, 0, 0, 0);
  panel.singleLED(DEBUG_KD, 0, 0, 0);
}

void displaySpeed(int lspeed, int rspeed){
  //Serial.println("display speed");
  if (lspeed < 0){
    panel.singleLED(DEBUG_LSPEED, 0, 0, abs(lspeed));
  } else {
    panel.singleLED(DEBUG_LSPEED, abs(lspeed), 0, 0);
  }

  if (rspeed < 0){
    panel.singleLED(DEBUG_RSPEED, 0, 0, abs(rspeed));
  } else {
    panel.singleLED(DEBUG_RSPEED, abs(rspeed), 0, 0);
  }
}

//When using the camera to detect objects such as colored blobs or april tags. This function is
//useful when only a single object is the target. The approximate position will be marked with an
//led turning white in the 8*4 panel of the NeoPixel LED Panel. Therefore, this function can be 
//called assuming that you have created the LED panel object, which in this code is named "panel".
//If the LED panel is called something else, just edit the code here
void displayTrackedObject(int cx, int cy, int w_res, int h_res){
  int lednum = 0;
  int vertshift = 0;
  panel.resetPanel();
  lednum = cx/(w_res/8); //because lednum is an int, it will handle flooring the value
  vertshift = cy/(h_res/4);
  lednum = lednum + 8*vertshift;
  panel.singleLED(lednum, 10, 10 , 10);
}

void debugPIDConstants(int lednum, double oldval, double newval){
  int r, g, b;
  if (newval > oldval){
    panel.singleLED(lednum, 0, 10, 0);
  } else if (newval < oldval){
    panel.singleLED(lednum, 10, 0, 0);
  } else { //equal
    panel.singleLED(lednum, 10, 10, 10);
  }
}

#endif
