Color LEDStrip::nearColorTo(Color color1, Color color2, byte fadeSpeed) {
	Color newColor = color1;

	// calculate step size based on the maximum remaining difference
	unsigned char maximumDifference = getMaximumDifference(color1, color2);

	newColor.r = nearValue(color1.r, color2.r, getStepSize(color1.r, color2.r, fadeSpeed, maximumDifference));
	newColor.g = nearValue(color1.g, color2.g, getStepSize(color1.g, color2.g, fadeSpeed, maximumDifference));
	newColor.b = nearValue(color1.b, color2.b, getStepSize(color1.b, color2.b, fadeSpeed, maximumDifference));
	newColor.a = nearValue(color1.a, color2.a, getStepSize(color1.b, color2.b, fadeSpeed, maximumDifference));
	return newColor;
}
Target SingleBeamScanner::findClose(int x,int y)
{
  std::cout << "findclose" << std::endl;
  std::vector<Element*> neighbours;
  //int x_arr[] = {x , x+1, x+1 ,x+1 ,x ,x-1 ,x-1 ,x-1};
  //int y_arr[] = {y-1 , y-1, y ,y+1 ,y+1 ,y+1 ,y ,y-1}; //Diagonalt

  int x_arr[] = {x  , x+1, x   ,x-1};
  int y_arr[] = {y-1, y  , y+1 ,y};


  for(int i=0;i<4;i++)
  {
    if(x_arr[i]<0 ||
       x_arr[i]>=polygon->nx ||
       y_arr[i]<0 ||
       y_arr[i]>=polygon->ny)
        continue;

    if(polygon->matrix[x_arr[i]][y_arr[i]]->getStatus() == 5)
      continue;

    if(polygon->matrix[x_arr[i]][y_arr[i]]->getStatus() == 0 ||
       polygon->matrix[x_arr[i]][y_arr[i]]->getStatus() == 1)
       neighbours.push_back(polygon->matrix[x_arr[i]][y_arr[i]]);
  }

  //std::cout << "neighbours: " << neighbours.size() << std::endl;
  //for(int i=0;i<neighbours.size();i++)
  //{
    //std::cout << "(" << neighbours.at(i)->getIndexX() << "," << neighbours.at(i)->getIndexY() << ")" << std::endl;
  //}

  //calulate the values of the neighbours
  double highestValue = -std::numeric_limits<double>::max();
  bool unscanned = false;
  int index = -1;

  double scanweight = 1;
  double nearweight = 1.2;//.07;
  double headingweight = 0.7;

  for(int i=0;i<neighbours.size();i++)
  {
    Element* n = neighbours.at(i);
    double scanVal = scanweight * scanValue(n->getIndexX(),n->getIndexY(),x,y,0);
    double nearVal = nearweight * nearValue(n->getIndexX(),n->getIndexY(),x,y,0);
    double headingVal = headingweight * headingValue(n->getIndexX(),n->getIndexY(),x,y);

    std::cout << "\nElement      : (" << n->getIndexX() << "," << n->getIndexY() << ")" << std::endl;
    std::cout << "scanValue    : " << scanVal << std::endl;
    std::cout << "nearValue    : " << nearVal << std::endl;
    std::cout << "headingValue : " << headingVal << std::endl;
    //std::cout << "highestValue:  " << highestValue << std::endl;

    double value = scanVal + nearVal + headingVal;

    if(!unscanned)
    {
      if(n->getStatus() == 0)
      {
        highestValue = value;
        index = i;
        unscanned = true;
        std::cout << "unscanned element found" << std::endl;
      }
      else if(value > highestValue && scanVal !=0)
      {
        highestValue = value;
        index = i;
        std::cout << "scanned element is the best so far" << std::endl;
      }
    }
    else
    {
      if(unscanned && n->getStatus() == 0 && value > highestValue)
      {
        highestValue = value;
        index = i;
        std::cout << "unscanned element is the best so far" << std::endl;
      }
    }
  }

  if(index != -1 && highestValue != 0)
  {
    std::cout << "Best target: (" << neighbours.at(index)->getIndexX() << "," << neighbours.at(index)->getIndexY() << ")" << std::endl;
    int status = 0; //sucsess
    return Target(neighbours.at(index)->getIndexX(),neighbours.at(index)->getIndexY(),status);
  }

  int status = 1; //failed to find a good target
  return Target(x,y,status);

}