#include "MatrixUltrasonicSensor.h"

bool MatrixUltrasonic::begin(){
	Wire.begin();
	i2cMUXSelect();
	delay(50);
	if(i2cReadData(Device_ID) == 0x47){
		i2cWriteData(Device_Control, 0x04); // reset
		delay(500);
		i2cWriteData(Device_Control, 0x02); // enable
		return true;
	}
	else{
		return false;
	}
}

void MatrixUltrasonic::setAnime(AnimeType Anime){
	i2cMUXSelect();
	if(Anime == None){
		i2cWriteData(Device_Control, 0x02);
	}
	else{
		i2cWriteData(Device_Control, 0x03);
	}
	i2cWriteData(Device_Anime, uint8_t(Anime));
}

void MatrixUltrasonic::setSR(int rate){
	i2cMUXSelect();
	rate -=1;
	if(rate > 63){
		i2cWriteData(Device_Sample_Rate, 63);
	}
	else if(rate < 0){
		i2cWriteData(Device_Sample_Rate, 0);
	}
	else{
		i2cWriteData(Device_Sample_Rate, rate);
	}
}

uint16_t MatrixUltrasonic::getDistance(){
	i2cMUXSelect();

	uint16_t data = (uint16_t)(i2cReadData(Device_Distance_H) << 8 | i2cReadData(Device_Distance_L));
	return data;
}

void MatrixUltrasonic::i2cMUXSelect(){
	switch (_ver)
	{
	case 2:
		Wire.beginTransmission(ADDR_PCA954X);
		Wire.write(_ch + 4);
		Wire.endTransmission(1);

		delay(10);
		break;
	case 3:
		Wire.beginTransmission(ADDR_PCA954X);
		Wire.write((1 << _ch));
		Wire.endTransmission(1);
		
		delay(10);
		break;
	default:
		break;
	}
}

void MatrixUltrasonic::i2cWriteData(UltrasonicRegType reg, uint8_t data){

	Wire.beginTransmission(MatrixUR_ADDR);

	Wire.write(reg);
	Wire.write(data);

	Wire.endTransmission(1);
}

uint8_t MatrixUltrasonic::i2cReadData(UltrasonicRegType reg){
	
	Wire.beginTransmission(MatrixUR_ADDR);
	Wire.write(reg);
	Wire.endTransmission(1);

	delay(1);

	Wire.requestFrom(MatrixUR_ADDR, 1);

	delay(1);

	return Wire.read();
}
