//#include "util.h"
//#include "config.h"
#include "dac.h"
#include <wiringPi.h>
#include <stdio.h>

MCP4725 xDac;
MCP4725 yDac;

Dac xyz;

int i2c_init() {

    wiringPiSetup();
	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.setVoltageNew(dacX * 3, 0);
	yDac.setVoltageNew(dacY * 3, 0);
}

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;
       fd = wiringPiI2CSetup(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) {
printf("A: %i", wiringPiI2CWrite(fd, output >> 8));
wiringPiI2CWrite(fd, (output % 16));

}


void MCP4725::setVoltageNew(int voltage, int persist) {
  // 2 byte array to hold 12bit data chunks
  int data[2];
 
  // limit check voltage
  voltage = (voltage > 4095) ? 4095 : voltage;
 
  // MCP4725 expects a 12bit data stream in two bytes (2nd & 3rd of transmission)
  data[0] = (voltage >> 8) & 0xFF; // [0 0 0 0 D12 D11 D10 D9 D8] (first bits are modes for our use 0 is fine)
  data[1] = voltage; // [D7 D6 D5 D4 D3 D2 D1 D0]
 
  // 1st byte is the register
/*
  if (persist) {
    wiringPiI2CWrite(fd, WRITEDACEEPROM);
  } else {
    wiringPiI2CWrite(fd, WRITEDAC);
  }
*/
 
  // send our data using the register parameter as our first data byte
  // this ensures the data stream is as the MCP4725 expects
  wiringPiI2CWriteReg8(fd, data[0], data[1]);
}