#include <Adafruit_NeoPixel.h>
#ifdef __AVR__
  #include <avr/power.h>
#endif

#include <vector>
#include "wagon.h"

#define PIN D2
#define NUMPIXELS 300

Adafruit_NeoPixel strip = Adafruit_NeoPixel(NUMPIXELS, PIN, NEO_GRB + NEO_KHZ800);



long lastPixelUpdate=0;
#define PIXELUPDATETIME 10
long lastRoutineUpdate=0;
#define ROUTINEUPDATETIME 10

long loopmillis=0;

uint8_t height[NUMPIXELS];
#define MAXHEIGHT 45

std::vector <Wagon> wagon_arr;
uint8_t maxid=0;


void setup() {
  Serial.begin(115200);

  for (int i=0;i<NUMPIXELS;i++){
    height[i]=0;
  }
  //Temporaer
  int h=0;
  for (int i=2;i<=30;i++){
    height[i]=h*43/(30-2);
    h++;
  }
  for (int i=31;i<(31+55);i++){ //gerade obene ebene
    height[i]=43;
  }
  h=30;
  for (int i=(31+55);i<(31+55+30);i++){
    height[i]=h*43/(30);
    h--;
  }

  h=0;
  for (int i=(31+55+30+40);i<=(31+55+30+40+15);i++){
    height[i]=h*23/(15);
    h++;
  }
  for (int i=(31+55+30+40+15);i<=(31+55+30+40+15+55);i++){ //gerade mittlere ebene
    height[i]=23;
  }
  
  h=20;
  for (int i=(31+55+30+40+15+55);i<=(31+55+30+40+15+55+20);i++){ //gerade mittlere ebene
    height[i]=h*23/20;
    h--;
  }


  h=0;
  for (int i=NUMPIXELS-30;i<NUMPIXELS;i++){
    height[i]=h*40/30;
    h++;
  }
  
  

  strip.begin();
  strip.setBrightness(150);
  strip.show(); // Initialize all pixels to 'off'
  Serial.println("Started");

  previewHeightmap();

  spawnWagon();
  spawnWagon();

}

void previewHeightmap(){
  for (int i=0;i<NUMPIXELS;i++){
    
    //uint32_t c=Wheel(height[i]*255/45);
    uint8_t b=height[i]*255/MAXHEIGHT;
    uint32_t c=strip.Color(255-b,b,0);
    strip.setPixelColor(i,c);
  }
  strip.show();
  delay(3000);
}

void spawnWagon(){
  //Wagon tmpr = Wagon(maxid++,NUMPIXELS,&strip, height, 35, 6, 0.5,0); //spawn new wagon
  Wagon tmpr = Wagon(maxid++,NUMPIXELS,&strip, height, random(30, 200), 6, random(0.2, 10)/10.0,0); //spawn new wagon
  
  wagon_arr.push_back(tmpr);
  Serial.println("Spawned Wagon");
}


void loop() {
  loopmillis=millis();

  //######################### Update LED Output
  if (lastPixelUpdate+PIXELUPDATETIME<loopmillis){ 
    lastPixelUpdate=loopmillis;

    for (int i=0;i<NUMPIXELS;i++){ //all black
      uint32_t c=strip.Color(0,0,0);
      strip.setPixelColor(i,c);
    }

    for (std::vector<Wagon>::iterator it = wagon_arr.begin(); it != wagon_arr.end(); ++it) //all wagons
    {
      Wagon & w = *it;
      w.updateGraphics();
    }

    strip.show();
  }


  if (lastRoutineUpdate+ROUTINEUPDATETIME<loopmillis-ROUTINEUPDATETIME){
    Serial.println("Behind!!!!!!!!!!");
  }
  //######################### Update Physics
  if (lastRoutineUpdate+ROUTINEUPDATETIME<loopmillis){
    lastRoutineUpdate=loopmillis;

    for (std::vector<Wagon>::iterator it = wagon_arr.begin(); it != wagon_arr.end(); ++it) //all wagons
    {
      Wagon & w = *it;
      w.updatePhysics(ROUTINEUPDATETIME);
      if (!w.alive())
      {
        it = wagon_arr.erase(it); // After erasing, it is now pointing the next element.
        --it;
        spawnWagon(); //spawn new one
      }
    }
  }

  
  
}

// Input a value 0 to 255 to get a color value.
// The colours are a transition r - g - b - back to r.
uint32_t Wheel(byte WheelPos) {
  WheelPos = 255 - WheelPos;
  if(WheelPos < 85) {
    return strip.Color(255 - WheelPos * 3, 0, WheelPos * 3);
  }
  if(WheelPos < 170) {
    WheelPos -= 85;
    return strip.Color(0, WheelPos * 3, 255 - WheelPos * 3);
  }
  WheelPos -= 170;
  return strip.Color(WheelPos * 3, 255 - WheelPos * 3, 0);
}