#include "color_tools.h"
#include <Arduino.h>

#define MAX_HUE 360.0

// Normalize a hue into [0, MAX_HUE)
static inline float wrapHue(float h)
{
  while (h >= MAX_HUE) h -= MAX_HUE;
  while (h < 0.0f)   h += MAX_HUE;
  return h;
}


/************************* CLASS - HUE PALLET DISPENSER ************************/

HUE_PALLET_DISPENSER::HUE_PALLET_DISPENSER(void){
  Initialize(0, 360, 6);
}

void HUE_PALLET_DISPENSER::Initialize(float hue, float range, int colSteps){
  // Clamp and normalize inputs
  this->range = constrain(range, 0.0f, (float)MAX_HUE);
  this->hueSteps = (colSteps < 0) ? 0 : colSteps;

  this->startHue = wrapHue(hue - (this->range * 0.5f));
  this->hueIndex = 0;

  if (this->hueSteps <= 1) {
    this->hueIncrement = 0.0f;
    this->currHue = wrapHue(hue);
  } else {
    this->hueIncrement = (this->range / (this->hueSteps - 1));
    this->currHue = this->startHue;
  }
}


int HUE_PALLET_DISPENSER::GetNextPalletHue(void){
  if (this->hueSteps > 1) {
    this->currHue = wrapHue(this->startHue + (this->hueIndex * this->hueIncrement));
    this->hueIndex = (this->hueIndex + 1) % this->hueSteps;
  }

  // Convert to integer hue in [0, 359]
  int out = (int)(this->currHue + 0.5f);
  if (out >= (int)MAX_HUE) out -= (int)MAX_HUE;
  if (out < 0) out = 0; // safety
  return out;
}

float HUE_PALLET_DISPENSER::PeekNextPalletHue(int hueOffset){
  float tempHue = wrapHue(this->startHue + ((this->hueIndex + hueOffset) * this->hueIncrement));
  return tempHue;
}

void HUE_PALLET_DISPENSER::SetHueIndex(int hueIndex){
  if (this->hueSteps > 0) {
    int n = this->hueSteps;
    int idx = hueIndex % n;
    if (idx < 0) idx += n;
    this->hueIndex = idx;
    if (this->hueSteps > 1) {
      this->currHue = wrapHue(this->startHue + (this->hueIndex * this->hueIncrement));
    }
  } else {
    this->hueIndex = 0;
  }
}
