#include "util.h"
#include "config.h"
#include "dac.h"

MCP4725 xDac;
MCP4725 yDac;

Dac xyz;

int i2c_init() {

	if (!bcm2835_init())
		return (1);

	bcm2835_i2c_set_baudrate(3400000);

	return (0);
}

void Dac::Setup(void) {
	xDac.begin(0x61);
	yDac.begin(0x60);

	i2c_init();

}

void Dac::SetXY(int dacX, int dacY) {
	//xDac.setVoltageFast(dacX * 3, false);
	//yDac.setVoltageFast(dacY * 3, false);

	//Fast
	xDac.setVoltageFast(dacX * 3);
	yDac.setVoltageFast(dacY * 3);
}

void Dac::SetZ(uint8_t z) {
	PORT_ZDAC = (z) ? (PORT_ZDAC & ~ZDAC_BV) : (PORT_ZDAC | ZDAC_BV);
}

MCP4725::MCP4725() {
}

void MCP4725::begin(uint8_t addr) {
	_i2caddr = addr;
}

void MCP4725::setVoltage(uint16_t output, bool writeEEPROM) {
	bcm2835_i2c_begin();
	bcm2835_i2c_setSlaveAddress(_i2caddr);

	i2c_start_wait(_i2caddr);

	if (writeEEPROM)
		bcm2835_i2c_write(MCP4726_CMD_WRITEDACEEPROM);
	else
		bcm2835_i2c_write(MCP4726_CMD_WRITEDAC);

	bcm2835_i2c_write(output / 16); // Upper data bits          (D11.D10.D9.D8.D7.D6.D5.D4)
	bcm2835_i2c_write((output % 16) << 4);

	bcm2835_i2c_end();

}

void MCP4725::setVoltageFast(uint16_t output) {
	bcm2835_i2c_begin();
	bcm2835_i2c_setSlaveAddress(_i2caddr);
	bcm2835_i2c_write(volt >> 8);
	bcm2835_i2c_write (volt);
	bcm2835_i2c_end();

}