#include "ws2812b.h"
#include "spectrum.h"

static uint8_t saturate (const unsigned k, const unsigned ofs = 0x100u) {
  if (k < ofs) return 0u;             // začátek je třeba trochu posunout
  const unsigned x = k - ofs;         // jinak ledky trochu svítí - žárovka tuto vlastnost nemá
  if (x >= (1u << 16)) return 0xffu;  // zasarutuj
  unsigned w, y = 0xffu;              // aproximaci začneme od 255
  for (unsigned n=0u; n<6u; n++) {    // končí cca za 3-4 iterace, max. 6
    w = (y + x/y) >> 1; // Newtonova metoda sqrt
    if  (y == w) break; // konec iterace - lepší už to nebude
    else y = w;
  }
  return y;             // výsledek
}

unsigned int Spectrum::Send(uint16_t * const ptr, const unsigned int len) {
  led << true;
  // procesor je rychlý, zvládne FFT i s kosinovým oknem v přerušení
  CopyToComplex (buffer, ptr, len);
  ifft.Forward  (buffer);
  // vyhodnotí barvy pro basy, středy a výšky
  unsigned qa = 0u;
  for (unsigned n=1u;   n<blow;     n++) qa += buffer[n].norm();
  cred   = saturate (qa >> 0, 0x200u);
  qa = 0u;
  for (unsigned n=blow; n<bmiddle;  n++) qa += buffer[n].norm();
  cgreen = saturate (qa >> 4);
  qa = 0u;
  for (unsigned n=bmiddle; n<bhigh; n++) qa += buffer[n].norm();
  cblue  = saturate (qa >> 2);
  // uloží barvy do ledek
  Entry l0(0), l1(0), l2(0), l3(0);
  l0.ws.order = 0; l0.ws.r = cred;
  l1.ws.order = 1; l1.ws.g = cgreen;
  l2.ws.order = 2; l2.ws.b = cblue;
  l3.ws.order = 3; l3.ws.r = 0xffu - cred; l3.ws.g = 0xffu - cgreen; l3.ws.b = 0xffu - cblue;
  // a frontou pošle na výstup
  ring.Write (l0.number);
  ring.Write (l1.number);
  ring.Write (l2.number);
  ring.Write (l3.number);
  // změří čas výpočtu (pod 0.5ms, rámec je 32ms)
  led << false;
  return len;
}