#ifndef WS2812B_H
#define WS2812B_H

#include "../../io/fastGPIO.h"

struct Color {

	uint8_t r;
	uint8_t g;
	uint8_t b;

	Color() : r(0), g(0), b(0) {
		;
	}

	Color(const uint8_t r, const uint8_t g, const uint8_t b) : r(r), g(g), b(b) {
		;
	}

	void setRGB(const uint8_t r, const uint8_t g, const uint8_t b) {
		this->r = r;
		this->g = g;
		this->b = b;
	}

	void setOff() {
		this->r = 0;
		this->g = 0;
		this->b = 0;
	}

	void setHSV(const uint8_t h, const uint8_t s, const uint8_t v) {

		uint8_t region, remainder, p, q, t;

		region = h / 43;
		remainder = (h - (region * 43)) * 6;

		p = (v * (255 - s)) >> 8;
		q = (v * (255 - ((s * remainder) >> 8))) >> 8;
		t = (v * (255 - ((s * (255 - remainder)) >> 8))) >> 8;

		switch (region) {
			case 0:
				r = v; g = t; b = p;
				break;
			case 1:
				r = q; g = v; b = p;
				break;
			case 2:
				r = p; g = v; b = t;
				break;
			case 3:
				r = p; g = q; b = v;
				break;
			case 4:
				r = t; g = p; b = v;
				break;
			default:
				r = v; g = p; b = q;
				break;
		}

	}

};

template <int numLEDs> class WS2812B {

	#define LED_SET_PIN_TO_OUTPUT		GPIO5_OUTPUT_SET
	#define LED_SET_PIN_H				GPIO5_H
	#define LED_SET_PIN_L				GPIO5_L

	//#define NS_PER_TICK   ( (1000ul*1000ul*1000ul) / (80ul*1000ul*1000ul) )

	/** color-value for each attached LED */
	Color colors[numLEDs];

	/** enable/disable each led */
	bool enabled[numLEDs] = {true};

  public:

	/** ctor */
	WS2812B() {
		LED_SET_PIN_TO_OUTPUT;
	}

	/** set the color for the given LED */
	void setColor(const uint8_t idx, const Color rgb) {
		colors[idx] = rgb;
	}

	/** enable/disable the given LED */
	void setEnabled(const uint8_t idx, const bool en) {
		enabled[idx] = en;
	}

	/** is the given LED enabled? */
	bool isEnabled(const uint8_t idx) const {
		return enabled[idx];
	}

	Color& getColor(const uint8_t idx) {
	  return colors[idx];
	}

	/** flush configured changes */
	void flush() {

		LED_SET_PIN_TO_OUTPUT;

	  //cli();

	  //reset();

/*
	  send0();
	  send1();
	  send0();
	  send1();
	  send0();
	  send1();
	  send0();
	  send1();
	  */
	  /*
			send0();
			send0();
			send0();
			send0();
			send0();
			send0();
			send0();
			send0();

			send1();
			send1();
			send1();
			send1();
			send1();
			send1();
			send1();
		//     send0();

		//  send0();
	  */

		// process each LED
		for (uint8_t i = 0; i < numLEDs; ++i) {

			// send each LEDs 24-bit GRB data
			if (enabled[i]) {
				const Color rgb = colors[i];
				sendByte(rgb.g);
				sendByte(rgb.r);
				sendByte(rgb.b);
			} else {
				sendByte(0);
				sendByte(0);
				sendByte(0);
			}

		}

		reset();


	 // sei();


	}

  private:

	inline void sendByte(uint8_t b) {
	  for (uint8_t i = 0; i < 8; ++i) {
		if (b & 0b10000000) {
		  send1();
		} else {
		  send0();
		}
		b <<= 1;
	  }
	}

	__attribute__((always_inline)) inline void send1() {               // 800ns high, 450ns low
		LED_SET_PIN_H;
		delay800();
		LED_SET_PIN_L;
		delay100();
	}

	__attribute__((always_inline)) inline void send0() {               // 400ns high, 850ns low
		LED_SET_PIN_H;
		delay100();
		LED_SET_PIN_L;
		delay800();
	}

	__attribute__((always_inline)) inline void reset() {
		LED_SET_PIN_L;
		os_delay_us(50);
		//delayMicroseconds(50);
	}


	//#pragma GCC optimize 0

	__attribute__((always_inline)) inline void delay50() {
	  asm volatile("nop");
	  asm volatile("nop");
	  asm volatile("nop");
	  asm volatile("nop");
	}


	// 100 ns delay. @80 MHz one nop = 12,5ns
	__attribute__((always_inline)) inline void delay100() {
	  delay50();
	  delay50();
	}

	__attribute__((always_inline))  inline void delay200() {
	  delay100();
	  delay100();
	}

	__attribute__((always_inline)) inline void delay400() {
	  delay100();
	  delay100();
	  delay100();
	  delay100();
	}

	__attribute__((always_inline)) inline void delay500() {
	  delay100();
	  delay100();
	  delay100();
	  delay100();
	  delay100();
	}

	__attribute__((always_inline)) inline void delay600() {
	  delay100();
	  delay100();
	  delay100();
	  delay100();
	  delay100();
	  delay100();
	}

	__attribute__((always_inline)) inline void delay800() {
	  delay400();
	  delay400();
	}


	//  #pragma GCC reset_options



	//  inline void delayNS(const uint16_t ns) {
	//    const uint16_t ticks = ns / NS_PER_TICK / 2;
	//    for (uint16_t i = 0; i < ticks; ++i) {asm("nop");}
	//  }

};

#endif // WS2812B_H