#include "line.h"

//uint8_t mass[6] = {4, 4, 2, 2, 4, 4};
uint8_t mass[6] = {1, 1, 1, 1, 1, 1};

int64_t line_reg(LineSensors *sensors)  //line regulator function
{
    static double _lastValue;
    static uint64_t _lastTime = 0;
	static uint32_t _colorTime = 0;
    
    bool on_line = 0; //if robot on line
    volatile int64_t avg = 0; // this is for the weighted total, which is long
                       // before division
    int32_t sum = 0; // this is for the denominator which is <= 64000

    int16_t sensor_values[6];
    
    for(uint8_t  i=0; i<6; i++)
    {
        sensor_values[i] = sensors->getValue(i); //sensors read form 0 to 1000
    }
	
	if(time_service::millis() - _colorTime > ((256 - TCS34725_INTEGRATIONTIME_2_4MS) * 12 / 5 + 1))
	{
		if(sensors->left < 0) sensors->left++;
		if(sensors->right < 0) sensors->right++;
		//bool flag = sensors->isRightOnGreen();
		if(sensor_values[1] > 350  && sensor_values[2] > 350 && sensors->isLeftOnGreen())
		{
			sensors->left++; //left green marker counter
		}
		else
		{
			if(sensors->left > 0) sensors->left--;
		}
		
		if(sensor_values[3] > 350  && sensor_values[4] > 350 && sensors->isRightOnGreen())
		{
			sensors->right++; //right green marker counter
		}
		else
		{
			if(sensors->right > 0) sensors->right--;
		}
		_colorTime = time_service::millis();
	}
	
	bool l = (sensor_values[0] < 10 && sensor_values[2] < 10);
	bool r = (sensor_values[3] < 10 && sensor_values[5] < 10);
	if(l && r)
	{
		sensors->silver++; //silver line counter
	}
	else
	{
		if(sensors->silver > 0) sensors->silver--;
	}
	
	if(l != r) //!= ---- xor
	{
		sensors->hill++;
	}
	else
	{
		if(sensors->hill > 0) sensors->hill--;
	}
	
	avg = 0;
	sum = 0;
    for(uint8_t i=0; i<6; i++) {
        int value = sensor_values[i];
        
        if(value > threshold) {
            on_line = 1;
        }

        if(value > 50) { //center of mass line regulator
            avg += (int64_t)(value) * ((int64_t)i * 1000) * (int64_t)mass[i];
            sum += (int32_t)(value)  * (int32_t)mass[i];
        }
    }

    if(!on_line)
    {
        //return (_lastValue - 2500.0) * k_p * 2;
		return 0;
    }

	if(time_service::micros() - _lastTime > 30000) _lastTime = time_service::micros() - 1000;
	//PD - regulator
    double u = (double(avg)/double(sum) - 2500.0) * line_k_p + (double(avg)/double(sum) - _lastValue) * line_k_d / (time_service::micros() - _lastTime);
    
    _lastValue = double(avg)/double(sum); //last error
    _lastTime = time_service::micros();

    return int64_t(u);
}

int16_t silver_reg(LineSensors *sensors)
{
	int16_t sensor_values[6];
    
    for(uint8_t  i=0; i<6; i++)
    {
        sensor_values[i] = sensors->getValue(i); //sensors read
    }
	
	int u = 6*(sensor_values[0] + sensor_values[1] + sensor_values[2] - sensor_values[3] - sensor_values[4] - sensor_values[5]);
	
	return u;
}

int16_t wall_reg(LineSensors *sensors, bool *wall)
{
	static uint64_t _lastTime = 0;
	static int16_t err_old = 0;
	if(time_service::micros() - _lastTime > 30000) _lastTime = time_service::micros();
	
	uint16_t dist = Laser::read();
	int16_t err = ((int16_t)dist - 140);
	int u = err * wall_k_p + (err - err_old) * wall_k_d / (time_service::micros() - _lastTime);
	if(dist > 300)
	{
		u = 0;
		*wall = false;
	}
	else
	{
		*wall = true;
	}

	_lastTime = time_service::micros();
	err_old = err;
	
	return u;
}
