#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <Adafruit_NeoPixel.h>

/*
 * Zum Hochladen auf das Pico-Platinchen Arduino Pro/Mini 
 * und ATmega328 8MHz 3.3V auswählen.
 */

/* Unser Streifen hat 161 LEDs */
#define NUMPIXELS      161
/* Dieser ist an den Pin 2 des Platinchens angeschlossen */
#define PIN              2
/* Farbe wechseln bei x deg/s */
#define CHANGE_ON       30
#define INTENSITY       15
/* Maximale Helligkeit */
#define MAX_BRIGHTNESS 255

/* Wie häufig aktualisiert werden soll */
#define BNO055_SAMPLERATE_DELAY_MS (30)

Adafruit_BNO055 bno = Adafruit_BNO055();
Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUMPIXELS, PIN, NEO_GRB + NEO_KHZ800);
boolean blink;

void setup(void) {
	Serial.begin(115200);
	pixels.begin();
	pixels.setPixelColor(0, 255, 255, 0);
	pixels.show();
	if (!bno.begin()) {
		/*
		 * Der Sensor konnte nicht initialisiert werden.
		 * Langsam rot hell/dunkel faden.
		 */
		while (true) {
			for (int16_t i = -255; i < 255; i++) {
				pixels.setPixelColor(0, 255-abs(i), 0, 0);
				pixels.show();
				delay(5);
			}
		}
	}
	/* Erfolgreich initialisiert, kurz grün blinken */
	for (uint8_t i = 1; i < 11; i++) {
		/* i&1 ist wie i%2 und gibt bei jeder ungeraden Zahl 1 aus, ansonsten 0.
		 * Dadurch blinkt der Pixel abwechselnd mit 255*1=255 und 255*0=0 */
		pixels.setPixelColor(0, 0, 255*(i&1), 0);
		pixels.show();
		delay(100);
	}
	/* Display the current temperature */
	int8_t temp = bno.getTemp();
	Serial.print("Current Temperature: ");
	Serial.print(temp);
	Serial.println(" C");
	Serial.println("");
	//bno.setExtCrystalUse(true);
}

void loop(void) {
	/*
	 * VECTOR_GYROSCOPE berechnet die Drehgeschwindigkeit in Grad/s.
	 * Lage des Pico-Platinchens:
	 *
	 * ______________________________________________________...
	 *                       ________________________________...
	 *     .-----------.    |___    ___________        ___|__...
	 *   / *Tx      D2* \   |GND)  |   _____   |  _   (GND|GN...
	 *  /                \  |___   |  /     \  | |  |  ___|__...
	 * ( *RX   PICO  GND* ) |DI ) <-DREHRICHTUNG->  | ( DO|DI...
	 *  \                /  |___   |  \_____/  | |__|  ___|  ...
	 *   \ *DTR    VCC* /   |5V+)  |___________|      (5V+|5V...
	 *    °------------°    |_____________________________|__...
	 * ______________________________________________________...
	 */
	imu::Vector<3> sens = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);

	uint8_t red = max(0, MAX_BRIGHTNESS - abs(sens.y() * INTENSITY));
	uint8_t green = min(MAX_BRIGHTNESS, max(0, abs(sens.y() * INTENSITY) - MAX_BRIGHTNESS/2));
	uint32_t color = pixels.Color(red, green, 0);
	/* blinken, wenn voll grün */
	if (abs(sens.y() * INTENSITY) - MAX_BRIGHTNESS >= 0) {
		/* nur alle BNO055_SAMPLERATE_DELAY_MS * 2 an-/aus-schalten */
		blink = (blink+1) & 3;
		if (blink > 1) {
			color = 0;
		}
	}
	for(int i=0; i<NUMPIXELS/3; i++) {
		pixels.setPixelColor(i*3, color);
	}
	pixels.show();
	Serial.println(abs(sens.y() * INTENSITY));
	delay(BNO055_SAMPLERATE_DELAY_MS);
}
