//============================================================================
//Copyright © 2012, open source hardware engineering
// 
//This application is free software; you can redistribute it and/or
//modify it under the terms of the GNU Lesser General Public
//License as published by the Free Software Foundation; either
//version 3 of the License, or (at your option) any later version.
//
//This application is distributed in the hope that it will be useful,
//but WITHOUT ANY WARRANTY; without even the implied warranty of
//MERCHANTABILITY or FITNESS FOR When PARTICULAR PURPOSE. See the GNU
//Lesser General Public License for more details.
//
//You should have received a copy of the GNU Lesser General Public
//License along with this library; if not, see <http://www.gnu.org/licenses/>
//
//=============================================================================

#include <LoadCell.h>
#include <utility/AD7730.h>

const unsigned int chopSlowestCmds[] = {
    COMMS_WRITE | FILTER_REG, 0x80, 0x00, FILTL_CHOP_ENA | SPI_LAST
};

const unsigned int initModeCmds[] = {
	COMMS_WRITE | MODE_REG, M_IDLE | MODEH_WORD_LEN_24, RNG_80_MV | SPI_LAST
};

const unsigned int startContinuousReadData[] = { COMMS_START_READ | DATA_REG | SPI_LAST };
const unsigned int stopContinuousReadData[] =  { COMMS_STOP_READ  | DATA_REG | SPI_LAST };

const unsigned int readDataReg[] = { 0, 0, SPI_LAST };

const unsigned int setSingleModeCmds[] = {
	COMMS_WRITE | MODE_REG, M_SINGLE | MODEH_WORD_LEN_24, RNG_80_MV | SPI_LAST
};

const unsigned int setOffsetDacCmds[] = {
	COMMS_WRITE | DAC_REG, SPI_LAST
};

const unsigned int readModeCmds[] = {
	COMMS_READ | MODE_REG, 0, SPI_LAST
};

const unsigned int writeModeCmds[] = {
	COMMS_WRITE | MODE_REG, 0, SPI_LAST
};

const byte defaultMode[] = { 0x01, 0xB0 };

const unsigned int readDataCmds[] = {
	COMMS_READ | DATA_REG, 0, 0, SPI_LAST
};

const unsigned int readStatusCmds[] = {
	COMMS_READ | STAT_REG, SPI_LAST
};

const unsigned int readFilterCmds[] = {
	COMMS_READ | FILTER_REG, 0, 0, SPI_LAST
};

const unsigned int writeFilterCmds[] = {
	COMMS_WRITE | FILTER_REG, 0, 0, SPI_LAST
};

const byte defaultFilter[] = { 0x20, 0x00, 0x10 };

const unsigned int readDacCmds[] = {
	COMMS_READ | DAC_REG, SPI_LAST
};

const unsigned int writeDacCmds[] = {
	COMMS_WRITE | DAC_REG, SPI_LAST
};

const byte defaultDac[] = { 0x20 };

const unsigned int readOffsetCmds[] = {
	COMMS_READ | OFFSET_REG, 0, 0, SPI_LAST
};

const unsigned int writeOffsetCmds[] = {
	COMMS_WRITE | OFFSET_REG, 0, 0, SPI_LAST
};

const byte defaultOffset[] = { 0x80, 0x00, 0x00 };

const unsigned int readGainCmds[] = {
	COMMS_READ | GAIN_REG, 0, 0, SPI_LAST
};

const unsigned int writeGainCmds[] = {
	COMMS_WRITE | GAIN_REG, 0, 0, SPI_LAST
};

const byte defaultGain[] = { 0x59, 0x3C, 0xEA };

LoadCellAD7730 LoadCell = LoadCellAD7730();

LoadCellAD7730::LoadCellAD7730()
{
}

LoadCellAD7730::~LoadCellAD7730()
{
}

bool LoadCellAD7730::init()
{
	return init(DEFAULT_RST_PIN, DEFAULT_STBY_PIN, DEFAULT_POL_PIN, DEFAULT_RDY_PIN, DEFAULT_SS_PIN);
}

bool LoadCellAD7730::init(byte rst_pin, byte stby_pin, byte pol_pin, byte rdy_pin, byte ss_pin)
{
	bool okay = true;

	reset_pin = rst_pin;
	standby_pin = stby_pin;
	clk_polarity_pin = pol_pin;
	ready_pin = rdy_pin;

	pinMode(reset_pin,OUTPUT);
	digitalWrite(reset_pin,LOW);
	delay(10);
	digitalWrite(reset_pin,HIGH);

	pinMode(standby_pin,OUTPUT);
	digitalWrite(standby_pin,HIGH);

	pinMode(clk_polarity_pin,OUTPUT);
	digitalWrite(clk_polarity_pin,HIGH);

	pinMode(ready_pin, INPUT);
	digitalWrite(ready_pin, HIGH); 

	WxSpi.begin(500000UL, (byte)3);
	WxSpi.declare_slave_pin(ss_pin);

	// WxSpi.write_read(chopSlowestCmds, spiReadBuffer, (byte)sizeof(spiReadBuffer));
	
	// check default contents of registers after reset to verify proper operation of the
	// AD7730 and SPI communication port.

	digitalWrite(reset_pin,LOW);
	delay(2);
	digitalWrite(reset_pin,HIGH);
	delay(2);

	do
	{
		WxSpi.write_read(readModeCmds, spiReadBuffer, sizeof(spiReadBuffer));
		if (memcmp(spiReadBuffer+1, defaultMode, sizeof(defaultMode)) != 0) 
		{
			Serial.print("Bad initial mode: ");
			Serial.print(spiReadBuffer[1],HEX);
			Serial.println(spiReadBuffer[2],HEX);
			okay=false; 
			break; 
		}

		WxSpi.write_read(readFilterCmds, spiReadBuffer, sizeof(spiReadBuffer));
		if (memcmp(spiReadBuffer+1, defaultFilter, sizeof(defaultFilter)) != 0) 
		{ 
			okay=false; 
			Serial.println("Bad initial filter");
			break; 
		}

		WxSpi.write_read(readDacCmds, spiReadBuffer, sizeof(spiReadBuffer));
		if (memcmp(spiReadBuffer+1, defaultDac, sizeof(defaultDac)) != 0) 
		{ 
			okay=false; 
			Serial.println("Bad initial DAC");
			break; 
		}
		
		WxSpi.write_read(readOffsetCmds, spiReadBuffer, sizeof(spiReadBuffer));
		if (memcmp(spiReadBuffer+1, defaultOffset, sizeof(defaultOffset)) != 0) 
		{ 
			okay=false; 
			Serial.println("Bad initial offset");
			break; 
		}

		WxSpi.write_read(readGainCmds, spiReadBuffer, sizeof(spiReadBuffer));
		if (memcmp(spiReadBuffer+1, defaultGain, sizeof(defaultGain)) != 0) 
		{ 
			okay=false; 
			Serial.println("Bad initial gain");
			break;
		}

		// test some bit patterns in the offset register, just for fun...

	} while (0);
		
	if (!okay) return false;
		
	WxSpi.write_read(initModeCmds, spiReadBuffer, sizeof(spiReadBuffer));
	WxSpi.write_read(chopSlowestCmds, spiReadBuffer, sizeof(spiReadBuffer));

	return okay;
}

void LoadCellAD7730::waitReady()
{	
	while (digitalRead(ready_pin) == LOW);	// wait for the part to go BUSY first
	while (digitalRead(ready_pin) == HIGH); // now wait for the operation to finish.
}

void LoadCellAD7730::reorderLong(byte *buf)
{
	byte tmp;
	tmp = buf[0];
	buf[0] = buf[3];
	buf[3] = tmp;

	tmp = buf[1];
	buf[1] = buf[2];
	buf[2] = tmp;
}

void LoadCellAD7730::setGain(long gain)
{
	byte buf[4];
	long *lbuf = (long*)buf;
	*lbuf = gain;

	memcpy(spiWriteBuffer, writeGainCmds, sizeof(writeGainCmds));
	spiWriteBuffer[1] |= buf[2];
	spiWriteBuffer[2] |= buf[1];
	spiWriteBuffer[3] |= buf[0];
	WxSpi.write_read(spiWriteBuffer, spiReadBuffer, sizeof(spiReadBuffer));
}

void LoadCellAD7730::setOffset(long offset)
{
	byte buf[4];
	long *lbuf = (long*)buf;
	*lbuf = offset;

	memcpy(spiWriteBuffer, writeOffsetCmds, sizeof(writeGainCmds));
	spiWriteBuffer[1] |= buf[2];
	spiWriteBuffer[2] |= buf[1];
	spiWriteBuffer[3] |= buf[0];
	WxSpi.write_read(spiWriteBuffer, spiReadBuffer, sizeof(spiReadBuffer));
}

bool LoadCellAD7730::tare(byte channel, int avg, long tareLimit, long *offset)
{
	Serial.println("* Measuring Tare...");	

	double foffset = 0.0;
	double ftare = 0.0;
	int k;
	long *lbuf = (long*)spiReadBuffer;
	int attempt;

	setRange(RNG_10_MV);

	for (attempt=0; attempt<5; attempt++)
	{
		if (attempt == 0)
		{
			foffset = 0.0;

			for (k=0; k<avg; k++)
			{
				setMode(M_SYS_ZERO_CAL);
				waitReady();	
		
				WxSpi.write_read(readOffsetCmds, spiReadBuffer, sizeof(spiReadBuffer));
				spiReadBuffer[0] = 0;
				reorderLong(spiReadBuffer);
				foffset += *lbuf;
			}
			*offset = (long)(foffset / avg + 0.5);
		}
		else
		{
			long delta = (long)ftare;
			*offset += delta;
		}

		setOffset(*offset);	

		ftare = 0.0; 

		for (k=0; k<avg; k++)
		{
			ftare += measure(channel, RNG_10_MV) - 8388608.0;
		}
		ftare = ftare / avg;

		if (abs(ftare) < tareLimit)
		{
			Serial.print("* Tare is OK: ");
			Serial.println(ftare,1);
			break;
		}
		
		Serial.print("* Tare exceeded limit: ");
		Serial.print(ftare,1);
		Serial.print(", offset = ");
		Serial.println(*offset);
	}
	
	Serial.print("* Offset = ");
	Serial.println(*offset,HEX);

	return true;
}

bool LoadCellAD7730::calibrate(byte channel, int avg, long tareLimit, long *gain, long *offset)
{
	double fgain = 0.0;
	int k;
	long *lbuf = (long*)spiReadBuffer;
	int attempt;

	Serial.println("* Calibrating gain...");

	setChannel(channel);
	setRange(RNG_80_MV);

	for (k=0; k<avg; k++)
	{
		setMode(M_INT_FS_CAL);
		waitReady();

		WxSpi.write_read(readGainCmds, spiReadBuffer, sizeof(spiReadBuffer));
		spiReadBuffer[0] = 0;	
		reorderLong(spiReadBuffer);
		// Serial.print(*lbuf,HEX); Serial.print(" : "); Serial.println(fgain);
		fgain += *lbuf;		
	}
	*gain = (long)(fgain / avg + 0.5);
	setGain(*gain);

	Serial.print("* Gain = "); Serial.print(*gain); Serial.print(" = "); Serial.println(*gain,HEX);

	tare(channel, avg, tareLimit, offset);

	return true;
}


void LoadCellAD7730::setMode(byte mode)
{
	WxSpi.write_read(readModeCmds, spiReadBuffer, (byte)sizeof(spiReadBuffer));

	// force mode bits into the MSB of the response

	spiReadBuffer[1] = (spiReadBuffer[1] & ~MODEH_MODE_MASK) | mode;
	
	memcpy(spiWriteBuffer, writeModeCmds, sizeof(writeModeCmds));
	spiWriteBuffer[1] |= spiReadBuffer[1];
	spiWriteBuffer[2] |= spiReadBuffer[2];

	WxSpi.write_read((const unsigned int*)spiWriteBuffer, spiReadBuffer, sizeof(spiReadBuffer));
}

void LoadCellAD7730::setRange(byte range)
{
	WxSpi.write_read(readModeCmds, spiReadBuffer, (byte)sizeof(spiReadBuffer));

	// force range bits into the LSB of the response

	spiReadBuffer[2] = (spiReadBuffer[2] & ~MODEL_RANGE_MASK) | range;

	memcpy(spiWriteBuffer, writeModeCmds, sizeof(writeModeCmds));
	spiWriteBuffer[1] |= spiReadBuffer[1];
	spiWriteBuffer[2] |= spiReadBuffer[2];

	WxSpi.write_read((const unsigned int*)spiWriteBuffer, spiReadBuffer, sizeof(spiReadBuffer));
}

void LoadCellAD7730::setChannel(byte ch)
{
	WxSpi.write_read(readModeCmds, spiReadBuffer, (byte)sizeof(spiReadBuffer));

	// force channnel setting into the LSB of the response

	spiReadBuffer[2] = (spiReadBuffer[2] & ~MODEL_CH_MASK) | ch;

	memcpy(spiWriteBuffer, writeModeCmds, sizeof(writeModeCmds));
	spiWriteBuffer[1] |= spiReadBuffer[1];
	spiWriteBuffer[2] |= spiReadBuffer[2];

	WxSpi.write_read((const unsigned int*)spiWriteBuffer, spiReadBuffer, sizeof(spiReadBuffer));
}

void LoadCellAD7730::setUnipolar(bool unipolar)
{
	WxSpi.write_read(readModeCmds, spiReadBuffer, (byte)sizeof(spiReadBuffer));

	spiReadBuffer[1] = (spiReadBuffer[1] & ~MODEH_UNIPOLAR) | (unipolar ? MODEH_UNIPOLAR : 0);
	memcpy(spiWriteBuffer, writeModeCmds, sizeof(writeModeCmds));
	spiWriteBuffer[1] |= spiReadBuffer[1];
	spiWriteBuffer[2] |= spiReadBuffer[2];

	WxSpi.write_read((const unsigned int*)spiWriteBuffer, spiReadBuffer, sizeof(spiReadBuffer));
}

long LoadCellAD7730::measure(byte channel, byte range)
{
	setChannel(channel);
	setRange(range);
	setMode(M_SINGLE);
	waitReady();
	WxSpi.write_read(readDataCmds, spiReadBuffer, sizeof(spiReadBuffer));
	
	spiReadBuffer[0] = 0;
	reorderLong(spiReadBuffer);
	long force = *((long*)spiReadBuffer);

	return force;
}

long LoadCellAD7730::measureAverage(byte channel, byte range, int avgCount, bool useFir)
{
	double sum = 0.0;
	double firSum = 0.0;
	double tapGain;
	int k;
	long *lbuf = (long*)spiReadBuffer;

	if (avgCount < 2) return 0L;

	int midPoint = avgCount >> 1;
	double sf = 1.0 / ((double)midPoint * (midPoint + 1.0));
	//Serial.println(sf,6);

	setChannel(channel);
	setRange(range);
	
	setMode(M_CONTINUOUS);
	//WxSpi.write_read(startContinuousReadData, spiReadBuffer, sizeof(spiReadBuffer));

	for (k=0; k<avgCount; k++)
	{
		waitReady();
		// WxSpi.write_read(readDataReg, spiReadBuffer+1, sizeof(spiReadBuffer));
		WxSpi.write_read(readDataCmds, spiReadBuffer, sizeof(spiReadBuffer));
		spiReadBuffer[0] = 0;
		reorderLong(spiReadBuffer);
		tapGain = (k >= midPoint) ? (midPoint - (k - midPoint)) : (k + 1);
		sum += (double)*lbuf;
		firSum += ((double)*lbuf) * tapGain;
	}
	// WxSpi.write_read(stopContinuousReadData, spiReadBuffer, sizeof(spiReadBuffer));
	setMode(M_IDLE);

	return useFir ? (long)(firSum * sf) : (long)(sum /avgCount);
}
