#include "../windentelemetry.h"
#include "tacho.h"
#include <ESP32Servo.h>
#include <Adafruit_NeoPixel.h>
Servo myservo;
Adafruit_NeoPixel pixels(NUMPIXELS, NEOPIN, NEO_GRB + NEO_KHZ800);
void init_servo(void)
{
  myservo.setPeriodHertz(50);
  myservo.attach(servoPin);
  set_tacho(0, 70, 150);
}

void set_tacho(int speed, int speed_min, int speed_max)
{
  int val = map(speed, -25, 220, 0, 155);
#ifdef DEBUG
  Serial.print("Tacho Mapping: ")
      Serial.println(val);
#endif
  myservo.write(val);
  pixels.clear();
  // normal speed
  if ((speed >= speed_min) && (speed <= speed_max))
  {
    int n;
    for (n = speed_min + 5; n < speed_max - 5; n = n + 10)
    {
      int i = map(n, 0, 220, 1, 12);
      pixels.setPixelColor(setoffsetpixel(i), pixels.Color(0, 150, 0));
    }
  }
  // knapp am minimum
  if ((speed <= speed_min + 5) && (speed >= speed_min))
  {
    int i = map(speed, 0, 220, 1, 12);
    pixels.setPixelColor(setoffsetpixel(i - 1), pixels.Color(150, 150, 0));
  }
  // knapp über max
  if ((speed <= speed_max) && (speed >= speed_max - 5))
  {
    int i = map(speed, 0, 220, 1, 12);
    pixels.setPixelColor(setoffsetpixel(i), pixels.Color(150, 0, 0));
  }
  // kleiner minimium
  if ((speed < speed_min) && (speed > 20))
  {
    int n;
    for (n = 0; n < speed_min; n = n + 10)
    {
      int i = map(n, 0, 220, 1, 12);
      pixels.setPixelColor(setoffsetpixel(i - 1), pixels.Color(150, 150, 0));
    }
  }
  // größer max
  if (speed > speed_max)
  {
    int n;
    for (n = speed_max; n <= 220; n = n + 10)
    {
      int i = map(n, 0, 220, 1, 12);
      pixels.setPixelColor(setoffsetpixel(i), pixels.Color(150, 0, 0));
    }
  }
  pixels.show();
}

void init_pixel(void)
{
  pixels.begin();
}

int setoffsetpixel(int pixel)
{
  int newpixel;
  newpixel = pixel + pixeloffset;
  if (newpixel >= NUMPIXELS)
  {
    newpixel = newpixel - NUMPIXELS;
  }
  return newpixel;
}