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