#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);

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

  for (int i=0;i<NUMPIXELS;i++){
    height[i]=255;
  }
  //Temporaer
  height[0]=46;
  height[28]=46;
  height[60]=3;
  height[63]=3;
  height[70]=4;
  height[100]=0;
  height[122]=0;
  height[137]=12;
  height[138]=12;
  height[152]=0;
  height[183]=0;
  height[200]=21;
  height[209]=26;
  height[225]=26;
  height[233]=23;
  height[240]=23;
  height[250]=21;
  height[255]=20;
  height[274]=46;
  height[NUMPIXELS-1]=46;

  //previewHeightmap(10000);

  //interpolate every part with height value 255
  for (int interpolateStartpos=0;interpolateStartpos<NUMPIXELS-1;interpolateStartpos++){
    if (height[interpolateStartpos]==255){ //interpolation part starts
      int interpolateEndpos=interpolateStartpos+1;
      while (interpolateEndpos<NUMPIXELS && height[interpolateEndpos]==255){
        interpolateEndpos++;
      }
      interpolateEndpos--;

      //interpolateStartpos index of first 255 value
      //interpolateEndpos index of last 255 value
      uint8_t interpolateStartvalue=height[interpolateStartpos-1];
      uint8_t interpolateEndvalue=height[interpolateEndpos+1];
      int interpolateLength=interpolateEndpos-interpolateStartpos+1; //one 255 element -> length 1
      float interpolateStep= ((int)(interpolateEndvalue)-(int)(interpolateStartvalue))*1.0 /(interpolateLength+1);
      Serial.println();
      Serial.print("interpolateStep=");
      Serial.print("(");
      Serial.print(interpolateEndvalue);
      Serial.print("-");
      Serial.print(interpolateStartvalue);
      Serial.print(")/");
      Serial.print(interpolateLength+1);
      Serial.print("=");
      Serial.println(interpolateStep);

      
      int interpolateStepCounter=1;
      Serial.println();
      Serial.print("interpolateStartpos=");
      Serial.println(interpolateStartpos);
      Serial.print("interpolateEndpos=");
      Serial.println(interpolateEndpos);
      Serial.print("interpolateStartvalue=");
      Serial.println(interpolateStartvalue);
      Serial.print("interpolateEndvalue=");
      Serial.println(interpolateEndvalue);
      Serial.print("interpolateLength=");
      Serial.println(interpolateLength);
      Serial.print("interpolateStep=");
      Serial.println(interpolateStep,6);
      for (int setinti=interpolateStartpos;setinti<=interpolateEndpos;setinti++) { //for all coherent elements to interpolate
        height[setinti]=height[interpolateStartpos-1]+(int)(interpolateStep*interpolateStepCounter);
        /*Serial.print(height[interpolateStartpos-1]);
        Serial.print("+(");
        Serial.print(interpolateStep);
        Serial.print("*");
        Serial.print(interpolateStepCounter);
        Serial.print(")=");
        Serial.println(height[setinti]);*/
        interpolateStepCounter++;
      }
      interpolateStartpos=interpolateEndpos;
    }
    
  }

  Serial.println();
  for (int i=0;i<NUMPIXELS;i++){
    Serial.print(i);
    Serial.print(": ");
    Serial.println(height[i]);
  }
  
  

  

  previewHeightmap(2000);

  spawnWagon();
  spawnWagon();

}

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

void spawnWagon(){
  //Wagon tmpr = Wagon(maxid++,NUMPIXELS,&strip, height, 35, 6, 0.5,0); //spawn new wagon
  //                                                      pos,      wagonlength,      startvel,           startacc,      wagonmass,      wagoncolor
  Wagon tmpr = Wagon(maxid++,NUMPIXELS,&strip, height, random(0, 20), random(3,20), random(0.2, 50)/10.0, 0           , random(5,40) , Wheel(random(0,255))); //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);
}