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

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

#define PIN D2
#define NUMPIXELS 600

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

long lastPixelUpdate=0;
#define PIXELUPDATETIME 20
long lastRoutineUpdate=0;
#define ROUTINEUPDATETIME 20
long lastCheckspawn=0;
#define CHECKSPAWNDELAY 4000 //delay in ms to check random spawn
#define SPAWNCHANCE 30 //1 out of x times wagon will spawn
#define SPAWNCHANCEDOUBLE 5 //change of spawning a two trains simultaneously

long loopmillis=0;

uint8_t height[NUMPIXELS];
uint8_t heightraw[NUMPIXELS]; //uninterpolated values
#define MAXHEIGHT 254

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

bool configmode=false;
int selectedpixel=-1; //-1 = none

uint8_t wagoncount=0;


//define config
//#define RESPAWNWAGON
#define MAXWAGONS 5 //maximum number of wagons


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

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

  resetHeightmap();
  
  //fixed heightmap
  heightraw[0]=254;
  heightraw[43]=254;
  heightraw[69]=200;
  heightraw[95]=149;
  heightraw[114]=132;
  heightraw[137]=128;
  heightraw[195]=128;
  heightraw[226]=150;
  heightraw[276]=139;
  heightraw[303]=150;
  heightraw[337]=131;
  heightraw[354]=129;
  heightraw[368]=131;
  heightraw[405]=172;
  heightraw[419]=147;
  heightraw[435]=117;
  heightraw[446]=105;
  heightraw[458]=96;
  heightraw[472]=77;
  heightraw[503]=35;
  heightraw[523]=0;
  heightraw[554]=0;
  heightraw[562]=8;
  heightraw[577]=34;
  heightraw[599]=67;



  interpolateHeightValues();

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

  

  //previewHeightmap(2000);

  //spawnWagon();
  //spawnWagon();

}

void resetHeightmap(){
  for (int i=0;i<NUMPIXELS;i++){
    heightraw[i]=255; //255 means value need to be interpolated
  }
  heightraw[0]=0;
  heightraw[NUMPIXELS-1]=0;
}

void printHeightmapRaw() {
  Serial.println();
  for (int i=0;i<NUMPIXELS;i++){
    if (heightraw[i]!=255){
      Serial.print("heightraw[");
      Serial.print(i);
      Serial.print("]=");
      Serial.print(heightraw[i]);
      Serial.println(";");
    }
  }
}

void interpolateHeightValues(){
  for (int i=0;i<NUMPIXELS;i++){ //copy heightraw to height
    height[i]=heightraw[i];
  }
  //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;
    }
  }
}

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);
    uint32_t c=Wheel(b/1.2);
    if (height[i]==255){
      c=strip.Color(0,0,0);
    }
    strip.setPixelColor(i,c);
  }
  
  if (waittime>0){
    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,      trainmass,      wagoncolor
  //Wagon tmpr = Wagon(maxid++,NUMPIXELS,&strip, height, random(0, 20), random(3,20), random(0.2, 50)/10.0, 0           , random(5,100) , Wheel(random(0,256))); //spawn new wagon
  Wagon tmpr = Wagon(maxid++,NUMPIXELS,&strip, height, random(0, 20), random(3,40), random(1, 70)/10.0, 0           , random(50,200) , Wheel(random(0,256))); //spawn new wagon
  
  wagon_arr.push_back(tmpr);
  Serial.println("Spawned Wagon");
}

void spawnWagon(float pos, float wagonlength,float startvel, float startacc, float mass, uint8_t wheelcolor){
  //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, pos, wagonlength,           startvel,              startacc       , mass , Wheel(wheelcolor)); //spawn new wagon
  
  wagon_arr.push_back(tmpr);
  Serial.println("Spawned Custom Wagon");
}


void loop() {
  loopmillis=millis();

  checkSerial();

  if (configmode){
    loop_configmode();
  }else{
    loop_achterbahn();
  }

}

void checkSerial(){
  static String serialstring_temp="";
  String serialstring="";
  while(Serial.available()){
    if (Serial.available()>0){
      char c = Serial.read();
      if (c!='\n') {
        serialstring_temp+=c;
        Serial.print(c); //echo
      }else{
        Serial.println(""); //new line
        serialstring=serialstring_temp;
        serialstring_temp="";
      }
    }
  }

  if (serialstring.length()>0) {
    Serial.println("String:"+serialstring);

    

    if (serialstring.equals("run")){
      configmode=false;
    }else if (serialstring.equals("debug")){
      configmode=true;
    }else if (serialstring.equals("print")){
      printHeightmapRaw();
    }else if (serialstring.equals("remove")){
      removeAllWagons();
    }else if (serialstring.equals("clear")){
      resetHeightmap();
      interpolateHeightValues();
      previewHeightmap(0); //show heightmap
      strip.show();
    }else if (serialstring.startsWith("spawn=")){
      String rest=serialstring.substring(serialstring.indexOf('=')+1); //part after =
      
      int spawnpos=rest.substring(0,rest.indexOf(',')).toInt(); //part to next ,
      rest=rest.substring(rest.indexOf(',')+1); //part after ,

      int spawnlength=rest.substring(0,rest.indexOf(',')).toInt(); //part to next ,
      rest=rest.substring(rest.indexOf(',')+1); //part after ,

      int spawnstartvel=rest.substring(0,rest.indexOf(',')).toInt(); //part to next ,
      rest=rest.substring(rest.indexOf(',')+1); //part after ,

      int spawnstartacc=rest.substring(0,rest.indexOf(',')).toInt(); //part to next ,
      rest=rest.substring(rest.indexOf(',')+1); //part after ,

      int spawnmass=rest.substring(0,rest.indexOf(',')).toInt(); //part to next ,
      rest=rest.substring(rest.indexOf(',')+1); //part after ,

      int spawncolor=rest.substring(0).toInt(); //part to next ,

      Serial.print("spawning ");
      Serial.print(spawnpos);
      Serial.print(",");
      Serial.println(spawnlength);
      Serial.print(",");
      Serial.println(spawnstartvel); //startvel will be /10
      Serial.print(",");
      Serial.println(spawnstartacc); //startacc will be /10
      Serial.print(",");
      Serial.println(spawnmass);
      Serial.print(",");
      Serial.println(spawncolor);
      
      spawnWagon(spawnpos,spawnlength,spawnstartvel/10.0,spawnstartacc/10.0,spawnmass,spawncolor);
    }else if (serialstring.equals("spawn")){
      spawnWagon(); //random
    }else if (serialstring.startsWith("setpx=")){
      String pixelnumberstring=serialstring.substring(serialstring.indexOf('=')+1,serialstring.indexOf(',')); //part between = and ,
      String pixelvaluestring=serialstring.substring(serialstring.indexOf(',')+1); //part after ,
      int pixelnumber=pixelnumberstring.toInt();
      int pixelvalue=pixelvaluestring.toInt();
      Serial.print("set pixel ");
      Serial.print(pixelnumber);
      Serial.print("=");
      Serial.println(pixelvalue);

      if (pixelnumber>=0 && pixelnumber<NUMPIXELS && pixelvalue>=0 && pixelvalue<=255){
        heightraw[pixelnumber]=pixelvalue;
      }else{
        Serial.println("Error: Value out of range!");
      }

      interpolateHeightValues();
      Serial.println();
      for (int i=0;i<NUMPIXELS;i++){
        Serial.print(i);
        Serial.print(": ");
        Serial.print(height[i]);
        Serial.print(" (");
        Serial.print(heightraw[i]);
        Serial.println(")");
      }
      previewHeightmap(0); //show heightmap
      strip.show();
    }else if (serialstring.startsWith("px=")){
      String pixelnumberstring=serialstring.substring(serialstring.indexOf('=')+1); //part between = and ,
      int pixelnumber=pixelnumberstring.toInt();

      Serial.print("show pixel ");
      Serial.print(pixelnumber);

      if (pixelnumber<NUMPIXELS){
        selectedpixel=pixelnumber;
      }else{
        Serial.println("Error: Value too high!");
      }
    }
    
  }
}

void loop_configmode(){
  if (lastPixelUpdate+PIXELUPDATETIME<loopmillis){ 
    lastPixelUpdate=loopmillis;
    
    previewHeightmap(0);

    if (selectedpixel>=0){
      uint32_t c=strip.Color(255,255,255);
      strip.setPixelColor(selectedpixel,c);
      if (selectedpixel>=1){
        uint32_t c=strip.Color(0,0,0);
        strip.setPixelColor(selectedpixel-1,c);
      }
      if (selectedpixel<NUMPIXELS-1){
        uint32_t c=strip.Color(0,0,0);
        strip.setPixelColor(selectedpixel+1,c);
      }
    }
    strip.show();
  }
}

void loop_achterbahn(){
  //######################### 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;

    wagoncount=0;
    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;
        #ifdef RESPAWNWAGON
          spawnWagon(); //spawn new one
        #endif
      }else{ //wagon is alive
        wagoncount++;
      }
    }
  }


  //Check spawning
  
  if (lastCheckspawn+CHECKSPAWNDELAY<loopmillis) {
    lastCheckspawn=loopmillis;
    Serial.print("Checking Spawning, wagons ");
    Serial.println(wagoncount);
    if (random(0,SPAWNCHANCE)==0 && wagoncount<MAXWAGONS) { //by chance, exclusive SPAWNCHANCE
      spawnWagon();
      if (random(0,SPAWNCHANCEDOUBLE)==0){
        spawnWagon();
      }
    }else{
      Serial.println("no spawn");
    }
    
    
  }

  
  
}

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

// 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);
}