diff --git a/achterbahn.ino b/achterbahn.ino
index 9fedab3..d602b64 100644
--- a/achterbahn.ino
+++ b/achterbahn.ino
@@ -7,23 +7,28 @@
 #include "wagon.h"
 
 #define PIN D2
-#define NUMPIXELS 300
+#define NUMPIXELS 600
 
 Adafruit_NeoPixel strip = Adafruit_NeoPixel(NUMPIXELS, PIN, NEO_GRB + NEO_KHZ800);
 
 long lastPixelUpdate=0;
-#define PIXELUPDATETIME 10
+#define PIXELUPDATETIME 20
 long lastRoutineUpdate=0;
-#define ROUTINEUPDATETIME 10
+#define ROUTINEUPDATETIME 20
 
 long loopmillis=0;
 
 uint8_t height[NUMPIXELS];
-#define MAXHEIGHT 45
+uint8_t heightraw[NUMPIXELS]; //uninterpolated values
+#define MAXHEIGHT 254
 
 std::vector <Wagon> wagon_arr;
 uint8_t maxid=0;
 
+bool configmode=true;
+int selectedpixel=-1; //-1 = none
+
+
 
 void setup() {
   Serial.begin(115200);
@@ -33,11 +38,10 @@ void setup() {
   strip.show(); // Initialize all pixels to 'off'
   Serial.println("Started");
 
-  for (int i=0;i<NUMPIXELS;i++){
-    height[i]=255;
-  }
+  resetHeightmap();
+  
   //Temporaer
-  height[0]=46;
+  /*height[0]=46;
   height[28]=46;
   height[60]=3;
   height[63]=3;
@@ -56,10 +60,42 @@ void setup() {
   height[250]=21;
   height[255]=20;
   height[274]=46;
-  height[NUMPIXELS-1]=46;
+  height[NUMPIXELS-1]=46;*/
 
-  //previewHeightmap(10000);
 
+  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 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
@@ -114,54 +150,194 @@ void setup() {
       }
       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);
+    //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);
   }
-  strip.show();
-  delay(waittime);
+  
+  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,      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
+  //                                                      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,255))); //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("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;
@@ -182,7 +358,7 @@ void loop() {
 
 
   if (lastRoutineUpdate+ROUTINEUPDATETIME<loopmillis-ROUTINEUPDATETIME){
-    Serial.println("Behind!!!!!!!!!!");
+    Serial.println("Behind!");
   }
   //######################### Update Physics
   if (lastRoutineUpdate+ROUTINEUPDATETIME<loopmillis){
@@ -205,6 +381,16 @@ void loop() {
   
 }
 
+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) {
diff --git a/achterbahnconfig/achterbahnconfig.pde b/achterbahnconfig/achterbahnconfig.pde
new file mode 100644
index 0000000..27bab49
--- /dev/null
+++ b/achterbahnconfig/achterbahnconfig.pde
@@ -0,0 +1,84 @@
+import controlP5.*;
+import processing.serial.*;
+
+ControlP5 cp5;
+
+Serial myPort;  // Create object from Serial class
+int val;        // Data received from the serial port
+
+float links;
+float rechts;
+float both;
+Slider slLinks;
+Slider slRechts;
+
+Slider slBoth;
+
+void setup() {
+  size(450, 300);
+  frameRate(100);
+  background(0);
+  
+  cp5 = new ControlP5(this);
+/*
+  slLinks = cp5.addSlider("links")
+     .setRange((float)-1, (float)1)
+     .setValue(0)
+     .setPosition(10,10)
+     .setSize(400,20)
+     ;
+
+  
+  slRechts = cp5.addSlider("rechts")
+     .setRange((float)-1,(float)1)
+     .setValue(0)
+     .setPosition(10,40)
+     .setSize(400,20);
+  
+     */
+  slBoth = cp5.addSlider("both")
+     .setRange((float)-1,(float)1)
+     .setValue(0)
+     .setPosition(10,20)
+     .setSize(400,20);
+     
+  cp5.addButton("stop")
+      .setPosition(10, 80)
+      .setSize(150, 150);
+
+    
+  myPort = new Serial(this, "/dev/ttyUSB0", 115200);
+}
+
+void stop() {
+  /*slLinks.setValue(0);
+  slRechts.setValue(0);*/
+  slBoth.setValue(0);
+}
+
+void draw() {
+  
+  //int bits = Float.floatToIntBits(rechts);
+  int bits = Float.floatToIntBits(both);
+  byte[] bytes = new byte[8];
+  bytes[0] = (byte)(bits & 0xff);
+  bytes[1] = (byte)((bits >> 8) & 0xff);
+  bytes[2] = (byte)((bits >> 16) & 0xff);
+  bytes[3] = (byte)((bits >> 24) & 0xff);
+  
+  //bits = Float.floatToIntBits(links);
+  bits = Float.floatToIntBits(both);
+  bytes[4] = (byte)(bits & 0xff);
+  bytes[5] = (byte)((bits >> 8) & 0xff);
+  bytes[6] = (byte)((bits >> 16) & 0xff);
+  bytes[7] = (byte)((bits >> 24) & 0xff);
+  
+  myPort.write(bytes);              // send an H to indicate mouse is over square
+  
+  
+  String inBuffer = myPort.readString();   
+  if (inBuffer != null) {
+    println(inBuffer);
+  }
+  
+}
\ No newline at end of file
diff --git a/achterbahnconfig/achterbahnconfig/achterbahnconfig.pde b/achterbahnconfig/achterbahnconfig/achterbahnconfig.pde
new file mode 100644
index 0000000..298eddc
--- /dev/null
+++ b/achterbahnconfig/achterbahnconfig/achterbahnconfig.pde
@@ -0,0 +1,188 @@
+import controlP5.*;
+import processing.serial.*;
+
+ControlP5 cp5;
+
+Serial myPort;  // Create object from Serial class
+
+int pixelnumber;
+int last_pixelnumber;
+
+int pixelvalue;
+
+Slider slPixel;
+Slider slPixelValue;
+
+int numpixels=600;
+int maxpixelvalue=254;
+
+Slider slSpawnPos;
+int spawnpos;
+Slider slSpawnLength;
+int spawnlength;
+int maxspawnlength=100;
+Slider slSpawnStartvel;
+int spawnstartvel;
+int maxspawnstartvel=100;
+Slider slSpawnStartacc;
+int spawnstartacc;
+int maxspawnstartacc=100;
+Slider slSpawnMass;
+int spawnmass;
+int maxspawnmass=200;
+Slider slSpawnColor;
+int spawncolor;
+int maxspawncolor=255;
+
+void setup() {
+  size(700, 300);
+  frameRate(100);
+  background(0);
+  
+  cp5 = new ControlP5(this);
+/*
+  slLinks = cp5.addSlider("links")
+     .setRange((float)-1, (float)1)
+     .setValue(0)
+     .setPosition(10,10)
+     .setSize(400,20)
+     ;
+
+  
+  slRechts = cp5.addSlider("rechts")
+     .setRange((float)-1,(float)1)
+     .setValue(0)
+     .setPosition(10,40)
+     .setSize(400,20);
+  
+     */
+  slPixel = cp5.addSlider("pixelnumber")
+     .setRange(0,numpixels-1)
+     .setValue(0)
+     .setPosition(10,20)
+     .setSize(numpixels,20);
+     
+  slPixelValue = cp5.addSlider("pixelvalue")
+     .setRange(0,maxpixelvalue)
+     .setValue(0)
+     .setPosition(10,50)
+     .setSize(maxpixelvalue,20);
+     
+  cp5.addButton("Set")
+      .setPosition(10, 80)
+      .setSize(60, 60);
+      
+  cp5.addButton("ClearHeightmap")
+      .setPosition(10, 80+60+5)
+      .setSize(100, 10);
+      
+  cp5.addButton("Remove")
+      .setPosition(100, 80)
+      .setSize(50, 50);
+      
+      
+  cp5.addButton("SpawnRandom")
+      .setPosition(160, 80+55)
+      .setSize(50, 30);    
+  
+  cp5.addButton("Spawn")
+      .setPosition(160, 80)
+      .setSize(50, 50);
+      
+  slSpawnPos = cp5.addSlider("spawnpos")
+     .setRange(0,maxpixelvalue)
+     .setValue(0)
+     .setPosition(220,80)
+     .setSize(maxpixelvalue,10);
+     
+  slSpawnLength = cp5.addSlider("spawnlength")
+     .setRange(0,maxspawnlength)
+     .setValue(5)
+     .setPosition(220,80+15*1)
+     .setSize(maxspawnlength,10);
+     
+  slSpawnStartvel = cp5.addSlider("spawnstartvel")
+     .setRange(0,maxspawnstartvel)
+     .setValue(10)
+     .setPosition(220,80+15*2)
+     .setSize(maxspawnstartvel,10);
+     
+  slSpawnStartacc = cp5.addSlider("spawnstartacc")
+     .setRange(0,maxspawnstartacc)
+     .setValue(0)
+     .setPosition(220,80+15*3)
+     .setSize(maxspawnstartacc,10);
+     
+  slSpawnMass = cp5.addSlider("spawnmass")
+     .setRange(1,maxspawnmass)
+     .setValue(10)
+     .setPosition(220,80+15*4)
+     .setSize(maxspawnmass-1,10);
+     
+  slSpawnColor = cp5.addSlider("spawncolor")
+     .setRange(0,maxspawncolor)
+     .setValue(0)
+     .setPosition(220,80+15*5)
+     .setSize(maxspawncolor,10);
+     
+     
+      
+  cp5.addButton("Debug")
+      .setPosition(10, 200)
+      .setSize(100, 30);
+      
+  cp5.addButton("Run")
+      .setPosition(10, 240)
+      .setSize(100, 30);
+
+    
+  myPort = new Serial(this, "/dev/ttyUSB0", 115200);
+}
+
+void Set() {
+  String writeserial="setpx="+pixelnumber+","+pixelvalue+"\n";
+  myPort.write(writeserial);
+}
+
+void ClearHeightmap(){
+  String writeserial="clear\n";
+  myPort.write(writeserial);
+}
+
+void Debug() {
+  String writeserial="debug\n";
+  myPort.write(writeserial);
+}
+
+void Run() {
+  String writeserial="run\n";
+  myPort.write(writeserial);
+}
+
+void Remove() {
+  String writeserial="remove\n";
+  myPort.write(writeserial);
+}
+
+void Spawn() {
+  //String writeserial="spawn\n"; //random
+  String writeserial="spawn="+spawnpos+","+spawnlength+","+spawnstartvel+","+spawnstartacc+","+spawnmass+","+spawncolor+"\n";
+  myPort.write(writeserial);
+}
+  
+
+void draw() {
+  
+  if (last_pixelnumber!=pixelnumber){
+    last_pixelnumber=pixelnumber;
+    String writeserial="px="+pixelnumber+"\n";
+    myPort.write(writeserial);
+  }
+  
+  String inBuffer = myPort.readString();   
+  if (inBuffer != null) {
+    println(inBuffer);
+  }
+  
+  
+}
\ No newline at end of file
diff --git a/wagon.cpp b/wagon.cpp
index 231a2d7..530caac 100644
--- a/wagon.cpp
+++ b/wagon.cpp
@@ -2,12 +2,12 @@
 
 #define SLOWVELOCITY 0.05
 
-Wagon::Wagon(int id,int numpixels, Adafruit_NeoPixel *strip,uint8_t *height,float pos, float wagonlength,float startvel,float startacc, float wagonmass, uint32_t wagoncolor)
+Wagon::Wagon(int id,int numpixels, Adafruit_NeoPixel *strip,uint8_t *height,float pos, float trainlength,float startvel,float startacc, float wagonmass, uint32_t wagoncolor)
 {
   _id = id;
   _numpixels=numpixels;
   _pos = pos;
-  _wagonlength = wagonlength;
+  _trainlength = trainlength;
   _strip=strip;
   _height=height;
   _vel=startvel;
@@ -28,10 +28,10 @@ void Wagon::updatePhysics(float updatedelayms)
   
   /*
   float acceleration=0;
-  for (int cpos=int(_pos);cpos>int(_pos-_wagonlength);cpos--){
+  for (int cpos=int(_pos);cpos>int(_pos-_trainlength);cpos--){
     acceleration=(_height[(int)cpos]-_height[(int)cpos+1])*0.03;
   }
-  acceleration/=int(_wagonlength);
+  acceleration/=int(_trainlength);
   _vel+= acceleration; //Acceleration from height difference
   _vel*=1-0.001; //resistance
   float airresistance=_vel*_vel *0.005;//air resistance
@@ -48,11 +48,11 @@ void Wagon::updatePhysics(float updatedelayms)
   //rho  Massendichte luft  1,2041
   //A = 1m^2
 
-  float m=_wagonmass/_wagonlength; //mass of part of a wagon
+  float m=_wagonmass/_trainlength; //mass of part of a wagon
 
   _acc=0;
   int cpos=(int)_pos;
-  for (int cpos=(int)_pos;cpos>(int)(_pos-_wagonlength);cpos--){ //for each wagon
+  for (int cpos=(int)_pos;cpos>(int)(_pos-_trainlength);cpos--){ //for each wagon
 
     float hdiff=getHeight((int) (cpos-0.5)) - getHeight((int)(cpos+0.5));
 
@@ -92,10 +92,10 @@ void Wagon::updatePhysics(float updatedelayms)
   
   _vel += _acc;
   _pos += _vel/PIXELDISTANCE;
-  Serial.print(" Vel=");
+  /*Serial.print(" Vel=");
   Serial.print(_vel);
   Serial.print(" Acc=");
-  Serial.println(_acc);
+  Serial.println(_acc);*/
 
   float _testvel=_vel;
   if (_testvel<0){
@@ -131,13 +131,13 @@ float Wagon::getHeight(int p){
 void Wagon::updateGraphics()
 {
   float wagonfeathering=2;
-  for(int i=_pos+wagonfeathering;i>_pos-_wagonlength-wagonfeathering;i--){
+  for(int i=_pos+wagonfeathering;i>_pos-_trainlength-wagonfeathering;i--){
     float featherbrightness=1;
     if (i>_pos){ //in front of wagon
       featherbrightness=1 - (i-_pos)/wagonfeathering;
       
-    }else if (i<_pos-_wagonlength){ //behind of wagon
-      featherbrightness=1 - (_pos-_wagonlength -i)/wagonfeathering;
+    }else if (i<_pos-_trainlength){ //behind of wagon
+      featherbrightness=1 - (_pos-_trainlength -i)/wagonfeathering;
     }
     
     if (featherbrightness<=0){ //distpercent between 0 and 1. 1-> full brightness, 0-> feathering distance away
diff --git a/wagon.h b/wagon.h
index 160161d..75de601 100644
--- a/wagon.h
+++ b/wagon.h
@@ -25,7 +25,7 @@ class Wagon
     float _vel;
     float _acc;
     float _wagonmass;
-    float _wagonlength;
+    float _trainlength;
     uint8_t *_height;
     long _spawntime;
     long _lasttimefast;