#include <FastLED.h>

#define LED_PIN  3

#define COLOR_ORDER GRB
#define CHIPSET     WS2811

#define BRIGHTNESS 255

#define NUM_LEDS 512//(kMatrixWidth * kMatrixHeight)
#define NUM_LEDS_TEIL 200//(kMatrixWidth * kMatrixHeight)

using LED_ARRAY = uint8_t[NUM_LEDS];
using TEIL_ARRAY = uint8_t [NUM_LEDS_TEIL];
using PIC_ARRAY = unsigned short [256];

const uint8_t kMatrixWidth = 16;
const uint8_t kMatrixHeight = 16;

// Param for different pixel layouts
const bool    kMatrixSerpentineLayout = true;
const bool    kMatrixVertical = false;

const unsigned short pic_heart[256] PROGMEM={
0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,   // 0x0010 (16) pixels
0x0000, 0x0000, 0x20C2, 0x832B, 0xAC50, 0x93CE, 0x3944, 0x0000, 0x0000, 0x3924, 0x82EA, 0x82EA, 0x59E6, 0x1861, 0x0000, 0x0000,   // 0x0020 (32) pixels
0x0000, 0x2903, 0xBCB1, 0xDDF6, 0xE658, 0xE658, 0xD595, 0x49A5, 0x4985, 0xC4B1, 0xCCD1, 0xBC4F, 0xA38C, 0x7289, 0x1881, 0x0000,   // 0x0030 (48) pixels
0x0000, 0x936C, 0xD533, 0xE5F6, 0xEE16, 0xEE17, 0xE5D6, 0xBC70, 0xB42F, 0xDD12, 0xD4D1, 0xCC4F, 0xB3AC, 0x8AEA, 0x59C6, 0x0000,   // 0x0040 (64) pixels
0x20A2, 0xB3ED, 0xD4D1, 0xE533, 0xE573, 0xE574, 0xE573, 0xDD12, 0xDCD0, 0xDC90, 0xD42E, 0xC3CD, 0xAB4B, 0x92C9, 0x6A27, 0x1861,   // 0x0050 (80) pixels
0x30E3, 0xAB4B, 0xCBED, 0xD44E, 0xDC6F, 0xDC6F, 0xDC6F, 0xDC2E, 0xD40D, 0xD3CC, 0xC38B, 0xB32A, 0x9AC9, 0x8267, 0x61E6, 0x2081,   // 0x0060 (96) pixels
0x20A1, 0x9267, 0xB2E9, 0xC329, 0xCB4A, 0xCB6A, 0xD34A, 0xCB29, 0xCB29, 0xC2E8, 0xB2A8, 0x9A67, 0x8206, 0x71C5, 0x5964, 0x1861,   // 0x0070 (112) pixels
0x0800, 0x6964, 0x91E5, 0xAA45, 0xB266, 0xBA66, 0xBA66, 0xBA66, 0xB245, 0xAA25, 0x91E5, 0x81A4, 0x7184, 0x6143, 0x48E2, 0x0800,   // 0x0080 (128) pixels
0x0000, 0x3060, 0x7122, 0x9162, 0xA1A3, 0xA9C3, 0xB1C3, 0xB1C3, 0xA9C3, 0x9983, 0x8162, 0x7122, 0x6902, 0x58E1, 0x2840, 0x0000,   // 0x0090 (144) pixels
0x0000, 0x0000, 0x4060, 0x70E0, 0x8921, 0xA141, 0xA161, 0xA161, 0x9941, 0x9121, 0x8121, 0x70E1, 0x60A0, 0x3860, 0x0000, 0x0000,   // 0x00A0 (160) pixels
0x0000, 0x0000, 0x0820, 0x4880, 0x78C0, 0x8900, 0x9920, 0x9921, 0x9921, 0x9100, 0x80E0, 0x70C0, 0x4880, 0x0820, 0x0000, 0x0000,   // 0x00B0 (176) pixels
0x0000, 0x0000, 0x0020, 0x1882, 0x4880, 0x80E0, 0x9900, 0xA121, 0xA121, 0x9920, 0x80E0, 0x5080, 0x1081, 0x0000, 0x0000, 0x0000,   // 0x00C0 (192) pixels
0x0000, 0x0000, 0x0000, 0x0000, 0x0841, 0x4080, 0x88E0, 0xA920, 0xA920, 0x88E0, 0x4080, 0x0840, 0x0000, 0x0000, 0x0000, 0x0000,   // 0x00D0 (208) pixels
0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x2040, 0x70C0, 0x70C0, 0x2840, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,   // 0x00E0 (224) pixels
0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0820, 0x0820, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,   // 0x00F0 (240) pixels
0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,   // 0x0100 (256) pixels
};


const unsigned short  pic_skull1[256] PROGMEM={
0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x2104, 0x4A69, 0x630C, 0x630C, 0x4A69, 0x2104, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,   // 0x0010 (16) pixels
0x0000, 0x0000, 0x0000, 0x10A2, 0x7BCF, 0xCE79, 0xEF5D, 0xFFDF, 0xFFDF, 0xEF5D, 0xCE79, 0x7BEF, 0x10A2, 0x0000, 0x0000, 0x0000,   // 0x0020 (32) pixels
0x0000, 0x0000, 0x18C3, 0xA514, 0xEF5D, 0xF7BE, 0xF7BE, 0xF7BE, 0xF7BE, 0xF7BE, 0xF7BE, 0xEF5D, 0xA534, 0x18C3, 0x0000, 0x0000,   // 0x0030 (48) pixels
0x0000, 0x0000, 0x9492, 0xE73C, 0xEF7D, 0xEF7D, 0xEF7D, 0xEF7D, 0xEF7D, 0xEF7D, 0xEF7D, 0xEF7D, 0xE73C, 0x9492, 0x0000, 0x0000,   // 0x0040 (64) pixels
0x0000, 0x4208, 0xD69A, 0xE73C, 0xE73C, 0xE73C, 0xE73C, 0xE73C, 0xE73C, 0xE73C, 0xE73C, 0xE73C, 0xE73C, 0xD69A, 0x4228, 0x0000,   // 0x0050 (80) pixels
0x0000, 0x8410, 0xDEFB, 0xE71C, 0xE71C, 0xE71C, 0xE71C, 0xE71C, 0xE71C, 0xE71C, 0xE71C, 0xE71C, 0xE71C, 0xDEFB, 0x8430, 0x0000,   // 0x0060 (96) pixels
0x0000, 0xAD75, 0xDEDB, 0xAD55, 0x5AEB, 0x8430, 0xDEDB, 0xDEDB, 0xDEDB, 0xDEDB, 0x8430, 0x5AEB, 0xAD55, 0xDEDB, 0xAD75, 0x0000,   // 0x0070 (112) pixels
0x0000, 0xB5B6, 0xCE59, 0x2945, 0x2124, 0x2124, 0x9CF3, 0xD6BA, 0xD6BA, 0x9CF3, 0x2124, 0x2124, 0x2945, 0xCE79, 0xB5B6, 0x0000,   // 0x0080 (128) pixels
0x0000, 0xB5B6, 0xC638, 0x2124, 0x2124, 0x2124, 0x94B2, 0xD69A, 0xD69A, 0x9492, 0x2124, 0x2124, 0x2124, 0xC638, 0xB5B6, 0x0000,   // 0x0090 (144) pixels
0x0000, 0xB596, 0xCE79, 0x8C51, 0x4208, 0x632C, 0xCE59, 0x94B2, 0x9CD3, 0xCE59, 0x632C, 0x4208, 0x8C51, 0xCE79, 0xB596, 0x0000,   // 0x00A0 (160) pixels
0x0000, 0xB596, 0xCE79, 0xCE79, 0xCE79, 0xCE79, 0xCE79, 0x4228, 0x4228, 0xCE79, 0xCE79, 0xCE79, 0xCE79, 0xCE79, 0xB596, 0x0000,   // 0x00B0 (176) pixels
0x0000, 0x9492, 0xCE59, 0xCE59, 0xCE59, 0xCE59, 0xCE59, 0xA534, 0xA534, 0xCE59, 0xCE59, 0xCE59, 0xCE59, 0xCE59, 0x9492, 0x0000,   // 0x00C0 (192) pixels
0x0000, 0x2124, 0x94B2, 0xB596, 0xB5B6, 0xCE59, 0xCE59, 0xCE59, 0xCE59, 0xCE59, 0xCE59, 0xB5B6, 0xB596, 0x94B2, 0x2124, 0x0000,   // 0x00D0 (208) pixels
0x0000, 0x0000, 0x0000, 0x0000, 0x73AE, 0xC638, 0xC618, 0xC638, 0xC638, 0xC618, 0xC638, 0x73AE, 0x0000, 0x0000, 0x0000, 0x0000,   // 0x00E0 (224) pixels
0x0000, 0x0000, 0x0000, 0x0000, 0x738E, 0xBDF7, 0xB596, 0xB5B6, 0xB5B6, 0xB596, 0xBDF7, 0x738E, 0x0000, 0x0000, 0x0000, 0x0000,   // 0x00F0 (240) pixels
0x0000, 0x0000, 0x0000, 0x0000, 0x31A6, 0x632C, 0x632C, 0x632C, 0x632C, 0x632C, 0x632C, 0x31A6, 0x0000, 0x0000, 0x0000, 0x0000,   // 0x0100 (256) pixels
};

const unsigned short  pic_stop[256] PROGMEM={
0x0820, 0xB1E7, 0xEA8A, 0xEA8A, 0x8965, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x8165, 0xEA8A, 0xEA8A, 0xB9E7, 0x1020,   // 0x0010 (16) pixels
0xB1C7, 0xEA8A, 0xEA8A, 0xEA8A, 0xEA8A, 0x8965, 0x0000, 0x0000, 0x0000, 0x0000, 0x8145, 0xEA8A, 0xEA8A, 0xEA8A, 0xEA8A, 0xB1E7,   // 0x0020 (32) pixels
0xEA69, 0xEA69, 0xEA69, 0xEA69, 0xEA69, 0xEA69, 0x8165, 0x0000, 0x0000, 0x8145, 0xEA69, 0xEA69, 0xEA69, 0xEA69, 0xEA69, 0xEA69,   // 0x0030 (48) pixels
0xDA49, 0xEA69, 0xEA69, 0xEA69, 0xEA69, 0xEA69, 0xEA69, 0x8165, 0x8145, 0xEA69, 0xEA69, 0xEA69, 0xEA69, 0xEA69, 0xEA69, 0xDA49,   // 0x0040 (64) pixels
0x6104, 0xDA49, 0xEA49, 0xEA69, 0xEA69, 0xEA69, 0xEA49, 0xEA49, 0xEA49, 0xEA49, 0xEA69, 0xEA69, 0xEA69, 0xEA49, 0xDA49, 0x6924,   // 0x0050 (80) pixels
0x0000, 0x6104, 0xDA49, 0xEA49, 0xEA49, 0xEA49, 0xEA49, 0xEA49, 0xEA49, 0xEA49, 0xEA49, 0xEA49, 0xEA49, 0xDA49, 0x6924, 0x0000,   // 0x0060 (96) pixels
0x0000, 0x0000, 0x6104, 0xDA28, 0xE249, 0xE249, 0xE249, 0xE249, 0xE249, 0xE249, 0xE249, 0xE249, 0xDA28, 0x6924, 0x0000, 0x0000,   // 0x0070 (112) pixels
0x0000, 0x0000, 0x0000, 0x6104, 0xDA28, 0xE228, 0xE228, 0xE228, 0xE228, 0xE228, 0xE228, 0xDA28, 0x6924, 0x0000, 0x0000, 0x0000,   // 0x0080 (128) pixels
0x0000, 0x0000, 0x0000, 0x7924, 0xE228, 0xE228, 0xE228, 0xE228, 0xE228, 0xE228, 0xE228, 0xE228, 0x8145, 0x0000, 0x0000, 0x0000,   // 0x0090 (144) pixels
0x0000, 0x0000, 0x7924, 0xE208, 0xE208, 0xE228, 0xE228, 0xE228, 0xE228, 0xE228, 0xE228, 0xE208, 0xE208, 0x8124, 0x0000, 0x0000,   // 0x00A0 (160) pixels
0x0000, 0x7904, 0xDA08, 0xE208, 0xE208, 0xE208, 0xE208, 0xE208, 0xE208, 0xE208, 0xE208, 0xE208, 0xE208, 0xDA08, 0x8124, 0x0000,   // 0x00B0 (176) pixels
0x7904, 0xDA08, 0xDA08, 0xDA08, 0xDA08, 0xDA08, 0xDA08, 0xD1E7, 0xD1E7, 0xDA08, 0xDA08, 0xDA08, 0xDA08, 0xDA08, 0xDA08, 0x7924,   // 0x00C0 (192) pixels
0xD9E7, 0xD9E7, 0xD9E7, 0xD9E7, 0xD9E7, 0xD9E7, 0xD1E7, 0x6104, 0x60E3, 0xC9E7, 0xD9E7, 0xD9E7, 0xD9E7, 0xD9E7, 0xD9E7, 0xD9E7,   // 0x00D0 (208) pixels
0xD1E7, 0xD9E7, 0xD9E7, 0xD9E7, 0xD9E7, 0xC9E7, 0x60E3, 0x0000, 0x0000, 0x60E3, 0xC9E7, 0xD9E7, 0xD9E7, 0xD9E7, 0xD9E7, 0xD1E7,   // 0x00E0 (224) pixels
0x8945, 0xD1C7, 0xD9C7, 0xD9C7, 0xC9C7, 0x60E3, 0x0000, 0x0000, 0x0000, 0x0000, 0x58E3, 0xC9C7, 0xD9C7, 0xD9C7, 0xD1C7, 0x8945,   // 0x00F0 (240) pixels
0x0800, 0x8945, 0xC9C7, 0xC1C7, 0x60E3, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x58E3, 0xC1C7, 0xC9C7, 0x8945, 0x0800,   // 0x0100 (256) pixels
};

// Seite, x, y, Breite, Höhe, ...Daten
TEIL_ARRAY array_Blink_Links = {
0,0,0,kMatrixWidth,4,
9,9,9,2,2,2,2,2,2,2,2,2,2,9,9,9,
9,9,2,2,2,2,2,2,2,2,2,2,2,2,9,9,
9,9,2,2,2,2,2,2,2,2,2,2,2,2,9,9,
9,9,9,2,2,2,2,2,2,2,2,2,2,9,9,9};

TEIL_ARRAY array_Blink_Rechts = {
1,0,0,kMatrixWidth,4,
9,9,9,2,2,2,2,2,2,2,2,2,2,9,9,9,
9,9,2,2,2,2,2,2,2,2,2,2,2,2,9,9,
9,9,2,2,2,2,2,2,2,2,2,2,2,2,9,9,
9,9,9,2,2,2,2,2,2,2,2,2,2,9,9,9};

TEIL_ARRAY array_Ruecklicht = {
2,0,8,kMatrixWidth,8,
3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,
3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,
3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,
3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,
3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,
3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,
3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,
3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3};

TEIL_ARRAY array_Rueckfahr = {
2,4,4,8,4,
1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,
};

TEIL_ARRAY array_Bremse = {
2,0,4,kMatrixWidth,4,
3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,
3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,
3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,
3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3};



CRGB leds[NUM_LEDS];


//PIN-EIngang

#define BLINK_LINKS 12
#define BLINK_RECHTS 11
#define BREMSE 10
#define RUECKFAHR 13
#define RUECKLICHT 9


int Status[40];
int StatusAlt[40];
int UpdateNoetig = 1;

int Mode = 0;
int Pic = 0;

void setup(void) {
  Serial.begin(57600);
  
  
  Serial.println("initializing ESP-NOW");
    
//    esp_now_register_recv_cb(data_receive);

  pinMode(BLINK_LINKS, INPUT_PULLUP);
  pinMode(BLINK_RECHTS, INPUT_PULLUP);
  pinMode(BREMSE, INPUT_PULLUP);
  pinMode(RUECKFAHR, INPUT_PULLUP);
  pinMode(RUECKLICHT, INPUT_PULLUP);


  FastLED.addLeds<CHIPSET, LED_PIN, COLOR_ORDER>(leds, NUM_LEDS).setCorrection(TypicalSMD5050);
  FastLED.setBrightness( BRIGHTNESS );

  for (int i=0 ; i<40 ; i++) {
    Status[i] = 0;
    StatusAlt[i] = 0;
  }

  for (int i=0 ; i<512 ; i++) {
    leds[i].setRGB(0,0,0);
  }
}


void loop() {
  int tempRead = 0;
  for (int i=0 ; i<40 ; i++) {
    StatusAlt[i]=Status[i];
    Status[i] = 0;
  }
  
  tempRead = digitalRead(BLINK_LINKS);
  if (tempRead == LOW) Status[BLINK_LINKS] = 1;
  if (Status[BLINK_LINKS] != StatusAlt[BLINK_LINKS]) UpdateNoetig = 1;
  
  tempRead = digitalRead(BLINK_RECHTS);
  if (tempRead == LOW) Status[BLINK_RECHTS] = 1;
  if (Status[BLINK_RECHTS] != StatusAlt[BLINK_RECHTS]) UpdateNoetig = 1;
  
  tempRead = digitalRead(BREMSE);
  
  if (tempRead == LOW) Status[BREMSE] = 1;
  if (Status[BREMSE] != StatusAlt[BREMSE]) UpdateNoetig = 1;
  
  tempRead = digitalRead(RUECKFAHR);
   if (tempRead == LOW) Status[RUECKFAHR] = 1;
  if (Status[RUECKFAHR] != StatusAlt[RUECKFAHR]) UpdateNoetig = 1;
  
  tempRead = digitalRead(RUECKLICHT);
  if (tempRead == LOW) Status[RUECKLICHT] = 1;
  if (Status[RUECKLICHT] != StatusAlt[RUECKLICHT]) UpdateNoetig = 1;

  if (UpdateNoetig == 1) UpdateDisplay();
}


void UpdateDisplay(void)
{
  UpdateNoetig = 0;
  for (int i=0; i<512; i++) leds[i]=0;
  
  //Segment Rücklicht
  if (Status[RUECKLICHT] == 1) {
    SetLED(array_Ruecklicht, true);
    Serial.println("UPDATE: setze Ruecklicht");
  
  }
  
  //Segment Rückfahr
  if (Status[RUECKFAHR] == 1) {
    SetLED(array_Rueckfahr, true);
    Serial.println("UPDATE: setze Rueckfahr");
  }
  
  //Segment Blinker links
  if (Status[BLINK_LINKS] == 1) {
    SetLED(array_Blink_Links, true);
    Serial.println("UPDATE: setze Blinker links");
  }
  
  //Segment Blinker rechts
  if (Status[BLINK_RECHTS] == 1) {
    SetLED(array_Blink_Rechts, true);
    Serial.println("UPDATE: setze Blinker rechts");
  }

  //Segment Bremse
  if (Status[BREMSE] == 1) {
    //SetLED(array_Bremse, true);
    SetPIC(2,pic_stop, true);
    Serial.println("UPDATE: setze Bremse");
  }
  
  FastLED.show();
}  

void SetLED(const TEIL_ARRAY &array, boolean on) 
{
  int i=0;
  int ia=5;
  int x1 = array[1];
  int y1 = array[2];
  int x2 = array[3] + x1;
  int y2 = array[4] + y1;
  int Seite = array[0];
  
  int R;
  int G;
  int B;
  
  for (int y=y1; y<y2; y++) {  
    for (int x=x1; x<x2; x++) {
      if( (y & 0x01) and (kMatrixSerpentineLayout==true) ) {
        // Odd rows run backwards
        uint8_t reverseX = (kMatrixWidth - 1) - x;
        i = (y * kMatrixWidth) + reverseX;
      } else {
        // Even rows run forwards
        i = (y * kMatrixWidth) + x;
      }
      /*Serial.print("array[ia]: "); Serial.println(array[ia]); 
      Serial.print("x: "); Serial.println(x); 
      Serial.print("y: "); Serial.println(y); 
      Serial.print("ia: "); Serial.println(ia); 
      Serial.print("i: "); Serial.println(i); 
      delay(100);
      */
      if (on  == true) {
        switch (array[ia]) { 
          case 1: R=255; G=255; B=255; break; //weiß
          case 2: R=255; G=255; B=0; break;   //gelb
          case 3: R=255; G=0; B=0; break;     //rot
          case 4: R=0; G=0; B=255; break;     //blau
          case 5: R=0; G=255; B=0; break;     //grün
        }
      }
      else {
        if (array[ia] !=0) R=0; G=0; B=0; 
      }

      if (array[ia] != 9){
        switch (Seite) {
          case 0: leds[i].setRGB(R,G,B); break;
          case 1: leds[i+256].setRGB(R,G,B); break; 
          case 2: leds[i].setRGB(R,G,B); leds[i+256].setRGB(R,G,B); break;
          //leds[i+Seite*kMatrixWidth*kMatrixHeight].setRGB(R,G,B); 
        }
    }
      ia++;
    }
  }
}

void SetPIC(int Seite, const PIC_ARRAY &array, boolean on) 
{
  int i=0;
  int ia=0;
  int x1 = 0;
  int y1 = 0;
  int x2 = 15;
  int y2 = 15;

  int R;
  int G;
  int B;
  
  for (int y=y1; y<y2+1; y++) {  
    for (int x=x1; x<x2+1; x++) {
      if( y & 0x01) {
        // Odd rows run backwards
        uint8_t reverseX = (kMatrixWidth - 1) - x;
        i = (y * kMatrixWidth) + reverseX;
      } else {
        // Even rows run forwards
        i = (y * kMatrixWidth) + x;
      }
      
      if (on  == true) {
        R = ((((array[ia] >> 11) & 0x1F) * 527) + 23) >> 6;
        G = ((((array[ia] >> 5) & 0x3F) * 259) + 33) >> 6;
        B = (((array[ia] & 0x1F) * 527) + 23) >> 6;
        switch (Seite) {
          case 0: leds[i].setRGB(R,G,B); break;
          case 1: leds[i+256].setRGB(R,G,B); break; 
          case 2: leds[i].setRGB(R,G,B); leds[i+256].setRGB(R,G,B); break;
          //leds[i+Seite*kMatrixWidth*kMatrixHeight].setRGB(R,G,B); 
        }
      }
      ia++;
    }
  }
  }