#include "LineSensorArray.h"

LineSensorArray::LineSensorArray(LineSensor *sensors[5])
{
	for(int i = 0; i < 5; i++)
		this->sensors[i] = sensors[i];
	
	line = 0.0;
	sharpness = 0.0;
	gw = 0.0;
	
	min_diff = 0.0;
	max_white_diff = 0.0;
}

LineSensorArray::LineSensorArray(LineSensor *sensor1, LineSensor *sensor2, LineSensor *sensor3, LineSensor *sensor4, LineSensor *sensor5)
{
	sensors[0] = sensor1;
	sensors[1] = sensor2;
	sensors[2] = sensor3;
	sensors[3] = sensor4;
	sensors[4] = sensor5;
	
	line = 0.0;
	sharpness = 0.0;
	gw = 0.0;
	
	min_diff = 0.0;
	max_white_diff = 0.0;
}

void LineSensorArray::update() {
	int values[5];
	int nBlack = 0;
	bool strange = false;
	
	
	for(int i = 0; i < 5; i++) {
		sensors[i]->updateValue();
		if((values[i] = int(isSensorBlack(i))) == 1)
			nBlack++;
		else if(nBlack > 0)
			strange = true;
	}
	
	if(nBlack == 0) {
		line = 0.0;
		sharpness = 0.0;
	}
	else {
		line = (-2*values[0]-1*values[1]+values[3]+2*values[4])/(float)nBlack;
	
		if(strange) sharpness = -1.0;
		else sharpness = 1/(float)nBlack;
	}
}

bool LineSensorArray::calibrate() {
	float white = (sensors[1]->getValue() + sensors[3]->getValue())/2;
	
	if((sensors[2]->getValue() - white) < min_diff) return false;
	if(ABS(sensors[1]->getValue() - sensors[3]->getValue()) > max_white_diff) return false;
	
	if(gw == 0.0)
		gw = sensors[2]->getValue()/2 + white/2;
	else
		gw = sensors[2]->getValue()/4 + white/4 + gw/2;
	
	return true;
}