#include "line_sensors.h"

int32_t map(int32_t x, int32_t in_min, int32_t in_max, int32_t out_min, int32_t out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

LineSensors :: LineSensors()
{
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2, ENABLE); //Periph clocking
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
    
    GPIO_InitTypeDef param; //port A configuration
    param.GPIO_Mode = GPIO_Mode_AN;
    param.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7;
    param.GPIO_PuPd = GPIO_PuPd_NOPULL;
    param.GPIO_OType = GPIO_OType_PP;
    param.GPIO_Speed = GPIO_Speed_100MHz;
    GPIO_Init(GPIOA, &param);
    
    param.GPIO_Mode = GPIO_Mode_AN; //port B configuration
    param.GPIO_Pin = GPIO_Pin_1;
    param.GPIO_PuPd = GPIO_PuPd_NOPULL;
    param.GPIO_OType = GPIO_OType_PP;
    param.GPIO_Speed = GPIO_Speed_100MHz;
    GPIO_Init(GPIOB, &param);
    
    param.GPIO_Mode = GPIO_Mode_OUT; //port B configuration
    param.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_12 | GPIO_Pin_13;
    param.GPIO_PuPd = GPIO_PuPd_NOPULL;
    param.GPIO_OType = GPIO_OType_PP;
    param.GPIO_Speed = GPIO_Speed_100MHz;
    GPIO_Init(GPIOB, &param);
    
    DMA_DeInit(DMA2_Stream0);
	
    DMA_InitTypeDef dma_param; //DMA2 configuration
    dma_param.DMA_Channel = DMA_Channel_0;
    dma_param.DMA_PeripheralBaseAddr = (uint32_t)&(ADC1->DR); //from ADC data register
    dma_param.DMA_Memory0BaseAddr = (uint32_t) &m_data[0]; //to buffer
    dma_param.DMA_DIR = DMA_DIR_PeripheralToMemory;
    dma_param.DMA_BufferSize = ADC_CHANNELS;
    dma_param.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
    dma_param.DMA_MemoryInc = DMA_MemoryInc_Enable;
    dma_param.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
    dma_param.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
    dma_param.DMA_Mode = DMA_Mode_Circular;
    dma_param.DMA_Priority = DMA_Priority_High;
    dma_param.DMA_FIFOMode = DMA_FIFOMode_Disable;
    dma_param.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull;
    dma_param.DMA_MemoryBurst = DMA_MemoryBurst_Single;
    dma_param.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
		
    DMA_Init(DMA2_Stream0, &dma_param); //DMA2 initialization
		
    DMA_Cmd(DMA2_Stream0, ENABLE);
    
    ADC_InitTypeDef adc_param; //ADC1 configuration
    ADC_CommonInitTypeDef adc_common;
		
    adc_common.ADC_Mode = ADC_Mode_Independent;
    adc_common.ADC_Prescaler = ADC_Prescaler_Div2;
    adc_common.ADC_DMAAccessMode = DISABLE;
    adc_common.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles;
    ADC_CommonInit(&adc_common);
		
    adc_param.ADC_ContinuousConvMode = ENABLE; //enable continious mode
    adc_param.ADC_ScanConvMode = ENABLE;
    adc_param.ADC_Resolution = ADC_Resolution_12b;
    adc_param.ADC_DataAlign = ADC_DataAlign_Right;
    adc_param.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T3_TRGO;
    adc_param.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_Rising;
    adc_param.ADC_NbrOfConversion = ADC_CHANNELS;
    ADC_Init(ADC1, &adc_param);
		
    ADC_DMARequestAfterLastTransferCmd(ADC1, ENABLE);
		
    ADC_EOCOnEachRegularChannelCmd(ADC1, ENABLE);
    
    ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_56Cycles); //ADC1 channels configuration
    ADC_RegularChannelConfig(ADC1, ADC_Channel_1, 2, ADC_SampleTime_56Cycles);
    ADC_RegularChannelConfig(ADC1, ADC_Channel_2, 3, ADC_SampleTime_56Cycles);
    ADC_RegularChannelConfig(ADC1, ADC_Channel_3, 4, ADC_SampleTime_56Cycles);
    ADC_RegularChannelConfig(ADC1, ADC_Channel_4, 5, ADC_SampleTime_56Cycles);
    ADC_RegularChannelConfig(ADC1, ADC_Channel_5, 6, ADC_SampleTime_56Cycles);
    ADC_RegularChannelConfig(ADC1, ADC_Channel_6, 7, ADC_SampleTime_56Cycles);
    ADC_RegularChannelConfig(ADC1, ADC_Channel_7, 8, ADC_SampleTime_56Cycles);
    ADC_RegularChannelConfig(ADC1, ADC_Channel_9, 9, ADC_SampleTime_56Cycles);
	
	m_wire1 = I2c(GPIOB, GPIO_Pin_12, GPIO_Pin_13);
	m_left = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_24MS, TCS34725_GAIN_60X);
	while(!m_left.begin(&m_wire1));
	
	m_wire2 = I2c(GPIOB, GPIO_Pin_12, GPIO_Pin_2);
	m_right = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_2_4MS, TCS34725_GAIN_60X);
	while(!m_right.begin(&m_wire2));
	
	left = 0;
	right = 0;
	silver = 0;
	hill = 0;
	
	for(int i=0; i<6; i++)
	{
		sensors_max[i] = 0;
		sensors_min[i] = 4096;
	}
	
	for(int i=0; i<4; i++)
	{
		left_max[i] = 0;
		right_max[i] = 0;
		left_min[i] = 10000;
		right_min[i] = 10000;
	}
}

void LineSensors :: start()
{
    ADC_Cmd(ADC1, ENABLE); //ADC1 starting
    ADC_DMACmd(ADC1, ENABLE);
    ADC_SoftwareStartConv(ADC1);
}

uint16_t LineSensors :: getRawValue(uint8_t channel)
{
    return m_data[channel];
}

int16_t LineSensors :: getValue(uint8_t channel)
{
	return map(m_data[channel], sensors_min[channel], sensors_max[channel], 0, 1000);
}

bool LineSensors :: isLeftOnGreen()
{
	/*uint16_t raw[4]; //raw rgbc values
	m_left.getRawData(&raw[0], &raw[1], &raw[2], &raw[3]);
	int16_t rgbc[4];
	for(int i=0; i<4; i++)
	{
		rgbc[i] = map(raw[i], left_min[i], left_max[i], 0, 1000);
		//rgbc[i] = raw[i];
	}*/
	/*int32_t sum = rgbc[3];
	for(int i=0; i<3; i++)
	{
		rgbc[i] = int32_t(rgbc[i]) * 100 / sum;
	}*/
	//double hue = calculateHue(rgbc[0], rgbc[1], rgbc[2]);
	float RGB[3];
	m_left.getRGB(&RGB[0], &RGB[1], &RGB[2]); 
	return (2*RGB[1] - RGB[0] - RGB[2] > 25);
}

bool LineSensors :: isRightOnGreen()
{
	/*uint16_t raw[4]; //raw rgbc values
	m_right.getRawData(&raw[0], &raw[1], &raw[2], &raw[3]);
	int16_t rgbc[4];
	for(int i=0; i<4; i++)
	{
		rgbc[i] = map(raw[i], right_min[i], right_max[i], 0, 1000);
	}*/
	/*int32_t sum = rgbc[0] + rgbc[1] + rgbc[2];
	for(int i=0; i<3; i++)
	{
		rgbc[i] = int32_t(rgbc[i]) * 1000 / sum;
	}*/
	//double hue = calculateHue(rgbc[0], rgbc[1], rgbc[2]);
	//return (80 < hue && hue < 130);
	float RGB[3];
	m_right.getRGB(&RGB[0], &RGB[1], &RGB[2]); 
	return (2*RGB[1] - RGB[0] - RGB[2] > 25);
}

bool LineSensors :: isCross()
{
	uint8_t sum = 0;
	for(uint8_t i=0; i<6; i++)
	{
		if(getValue(i) > 400) sum++;
	}
	return sum >= 5;
}

double LineSensors :: readSHARP()
{
	uint16_t value = m_data[6];
	
	double dist = 19500 * pow((double)value/4.0, -1.2045);
	return dist;
}

void LineSensors :: sensorsCalibration()
{
	for(int i=0; i<6; i++)
	{
		if(m_data[i] > sensors_max[i]) sensors_max[i] = m_data[i];
		if(m_data[i] < sensors_min[i]) sensors_min[i] = m_data[i];
	}
}

void LineSensors :: colorsCalibration()
{
	static uint32_t time = 0;
	if(time_service::millis() - time > ((256 - TCS34725_INTEGRATIONTIME_2_4MS) * 12 / 5 + 1))
	{
		uint16_t buf[4]; //rgbc values
		m_left.getRawData(&buf[0], &buf[1], &buf[2], &buf[3]);
		for(int i=0; i<4; i++)
		{
			if(buf[i] < left_min[i]) left_min[i] = buf[i];
			if(buf[i] > left_max[i]) left_max[i] = buf[i];
		}
		m_right.getRawData(&buf[0], &buf[1], &buf[2], &buf[3]);
		for(int i=0; i<4; i++)
		{
			if(buf[i] < right_min[i]) right_min[i] = buf[i];
			if(buf[i] > right_max[i]) right_max[i] = buf[i];
		}
		time = time_service::millis();
	}
}

double LineSensors :: calculateHue(int16_t r, int16_t g, int16_t b)
{
	int16_t max_v, min_v, delta;
	double h;
	
	min_v = min(r, min(g, b));
	max_v = max(r, max(g, b));
	delta = max_v - min_v;
	if(delta == 0) return 0;
	if(r == max_v)
	{
		h = double(g - b) / delta;
	}
	else if(g == max_v)
	{
		h = 2.0 + double(b - r) / delta;
	}
	else
	{
		h = 4.0 + double(r - g) / delta;
	}
	h *= 60;
	if(h < 0.0) h += 360.0;
	
	return h;
}
