#include "ultrasound.h"
#include "string.h"

UltrasoundHandleTypeDef ultrasound;

void ultrasound_init()
{
	memset(&ultrasound, 0, sizeof(UltrasoundHandleTypeDef));
	i2c_init();
}

uint16_t get_ultrasound_distance()
{
	uint8_t i;
	uint8_t distance[2] = {0};
	uint32_t sum = 0;
	
	const static uint8_t filter_num = 3;
	static uint16_t filter_buf[4] = {0};	/* filter_num + 1 */
	
	i2c_read_data(ULTRASOUND_ADDRESS << 1, ULTRASOUND_DISTANCE_REG, distance, sizeof(distance));
	filter_buf[filter_num] = (uint16_t)distance[1] << 8 | distance[0];
	/* µÝÍÆÆ½¾ùÂË²¨*/
	for (i = 0; i < filter_num; i++)
	{
		filter_buf[i] = filter_buf[i + 1];
		sum += filter_buf[i];
	}
	
	ultrasound.distance = (uint16_t)sum / filter_num;
	return ultrasound.distance;
}

void set_ultrasound_color(uint8_t mode, uint8_t* left_rgb, uint8_t* right_rgb)
{
	uint8_t i;
	uint8_t set_reg;
	uint8_t rgb[6];
	
	ultrasound.rgb_mode = mode;
	
	for (i = 0; i < 3; i++)
	{
		rgb[i] = left_rgb[i];
		rgb[i + 3] = right_rgb[i];
		ultrasound.left_rgb[i] = rgb[i];
		ultrasound.right_rgb[i] = rgb[i + 3];
	}
	
	switch(mode)
	{
		case 0:
			set_reg = RGB_WORK_SIMPLE_MODE_REG;
			i2c_write_data(ULTRASOUND_ADDRESS << 1, RGB_WORK_MODE_REG, &set_reg, 0);
			i2c_write_data(ULTRASOUND_ADDRESS << 1, RGB_CONSTANT_INDEX_REG, rgb, sizeof(rgb));
			break;
		
		case 1:
			set_reg = RGB_WORK_BREATHING_MODE_REG;
			i2c_write_data(ULTRASOUND_ADDRESS << 1, RGB_WORK_MODE_REG, &set_reg, 0);
			i2c_write_data(ULTRASOUND_ADDRESS << 1, RGB_BREATHING_INDEX_REG, rgb, sizeof(rgb));
			break;
		
		default:
			break;
	}
}
