   
     //to get the with 1,2,3 the needed indices we use i*2 and i*2+1
     //0 = 0,1 / 1 = 2,3 / 2 = 4,5   
-    int distance = (distanceValues[i*2] + distanceValues[i*2+1]) / 2;    
-    Serial.println(distance);
+    //int distance = (distanceValues[i*2] + distanceValues[i*2+1]) / 2;  
+    int distance = distanceValues[i];
+    if(distance < minDistance) {
+      if(distance == 0) {
+        distance = maxDistance; 
+      }  
+      else {
+        distance = minDistance;
+      }
+    }
+    //Serial.println(distance);
     int heightStrip = NUMBER_LEDS/NUMBER_STRIPS;
     
     //how many of the leds of one strip should be switched on

 *
 */
void loop(){
  int i = 0;
  for(servoPos = servoMinAngle; servoPos < servoMaxAngle; servoPos += servoStepAngle){
    readSensorWriteLEDs(servoPos, i);
    i+=1;
  }
  
  for(servoPos = servoMaxAngle; servoPos>servoMinAngle; servoPos-=servoStepAngle)
  {                
    readSensorWriteLEDs(servoPos, i);
    i-=1;
  }
}

void readSensorWriteLEDs(int servoPosition, int i){
  servo.write(servoPosition);
  delay(servoMSPerDegree*servoStepAngle);
  
  triggerSensor();
  int distance = readSensor();
  printDistance(distance);
  
  distance = max(minDistance, distance);
  distance = min(maxDistance, distance);
  distanceValues[i] = distance;

  visualizeWithDirection();

  delay(35);
}


/**

#include <Adafruit_NeoPixel.h>
#include <Servo.h> 
#include <avr/power.h>
#include <NewPing.h>

//Max and Min value for the distance sensor
float maxDistance = 400;
float minDistance = 10;

//Define 3 distance sensors
NewPing sonar1(11, 12, maxDistance); // Sensor 1: trigger pin, echo pin, maximum distance in cm
NewPing sonar2(9, 10, maxDistance); // Sensor 2: trigger pin, echo pin, maximum distance in cm
NewPing sonar3(7, 6, maxDistance); // Sensor 3: trigger pin, echo pin, maximum distance in cm

#define pingSpeed 100 // Ping frequency (in milliseconds), fastest we should ping is about 35ms per sensor
unsigned long pingTimer1, pingTimer2, pingTimer3;

//Hardware constants
#define PIN_STRIP 5
//#define PIN_SENSOR 7
//#define PIN_SERVO 8

#define NUMBER_LEDS 48
#define NUMBER_STRIPS 3

#define PIN_SWITCH 2

Adafruit_NeoPixel strip = Adafruit_NeoPixel(NUMBER_LEDS, PIN_STRIP, NEO_GRB + NEO_KHZ800);
//Servo servo;

//Servo variables
//int servoPos = 0;
//int servoSteps = 6;
//int servoMinAngle = 0;
//int servoMaxAngle = 90;
//int servoStepAngle = (servoMaxAngle-servoMinAngle)/(servoSteps-1);
//int servoMSPerDegree = 5;

//Store of the measured distances of each servo step
//int distanceValues[6];
  int distanceValues[3];

void setup() {  
  //Init distance store to far away
//  for(int i=0; i<servoSteps; i+=1){
//    distanceValues[i] = 0;
//  }
  distanceValues[0] = 0;
  distanceValues[1] = 0;
  distanceValues[2] = 0;
  
  pinMode(PIN_SWITCH, INPUT_PULLUP);           // set pin to input with internal pullup resistor
  
  //
  pingTimer1 = millis() + pingSpeed; // Sensor 1 fires after 100ms (pingSpeed)
  pingTimer2 = pingTimer1 + (pingSpeed / 2); // Sensor 2 fires 50ms later
  pingTimer3 = pingTimer2 + (pingSpeed / 2); // Sensor 3 fires 50ms later
  
  //For console output
  Serial.begin(9600);
