#include <msp430.h>
#include <msp430xgeneric.h>
#include <stdint.h>
#include <stdio.h>

#include "mw_main.h"
#include "mw_uart.h"

#include "mw_acc.h"

#include "oswald_main.h"

#define ACCEL_STATE_DISABLED		0x00
#define ACCEL_STATE_ENABLED		0x01

static uint8_t AccelState;
static uint8_t AccelerometerBusy;
static uint8_t LengthCount;
static uint8_t Index;
static uint8_t *pAccelerometerData;

/*
 * Accelerometer is a Kionix KXTF9-4100 connected to I2C
 * I2C is pretty slow so reading and writing should be done non blocking
 * using interrupts.
 */

#define ACCELEROMETER_NO_INTERRUPTS	0x00
#define ACCELEROMETER_ALIFG		0x02
#define ACCELEROMETER_NACKIFG		0x04
#define ACCELEROMETER_STTIFG		0x06
#define ACCELEROMETER_STPIFG		0x08
#define ACCELEROMETER_RXIFG		0x0a
#define ACCELEROMETER_TXIFG		0x0c

#pragma vector=USCI_B1_VECTOR
__interrupt void ACCERLEROMETER_I2C_ISR(void)
{
	// debug_uart_tx("ACC i2c irq\n");
	switch (USCI_ACCELEROMETER_IV) {
		case ACCELEROMETER_NO_INTERRUPTS: 
			break;
		case ACCELEROMETER_ALIFG: 
			break;
		case ACCELEROMETER_NACKIFG:
			nop();
			break; 
		case ACCELEROMETER_STTIFG:
			nop();
			break; 
		case ACCELEROMETER_STPIFG: 
			nop();
			break;
		case ACCELEROMETER_RXIFG:
			if (LengthCount > 0) {
				pAccelerometerData[Index++] = ACCELEROMETER_RXBUF;
				LengthCount--;
				if ( LengthCount == 1 ) {
					/* All but one byte received. Send stop */
					ACCELEROMETER_CTL1 |= UCTXSTP;
				} else if ( LengthCount == 0 ) {
					/* Last byte received; disable rx interrupt */
					ACCELEROMETER_IE &= ~UCRXIE;
					AccelerometerBusy = 0;
				}
			}
			break;
		case ACCELEROMETER_TXIFG:
			if ( LengthCount > 0 ) {
				ACCELEROMETER_TXBUF = pAccelerometerData[Index++];
				LengthCount--;
			} else {
				/* disable transmit interrupt and send stop */
				ACCELEROMETER_IE &= ~UCTXIE;
				ACCELEROMETER_CTL1 |= UCTXSTP;
				AccelerometerBusy = 0;
			}
			break;
		default:
			break;
	}

}

void mw_acc_init_i2c(void)
{
	/* enable reset before configuration */
	ACCELEROMETER_CTL1 |= UCSWRST;

	/* configure as master using smclk / 40 = 399.5 kHz */
	ACCELEROMETER_CTL0 = UCMST + UCMODE_3 + UCSYNC;
	ACCELEROMETER_CTL1 = UCSSEL__SMCLK + UCSWRST;
	ACCELEROMETER_BR0 = 42;

	ACCELEROMETER_BR1 = 0;
	ACCELEROMETER_I2CSA = KIONIX_DEVICE_ADDRESS;

	/* release reset */
	ACCELEROMETER_CTL1 &= ~UCSWRST;
}

void mw_acc_disable_i2c(void)
{
	/* enable reset to hold it */
	ACCELEROMETER_CTL1 |= UCSWRST;
}

void mw_acc_i2c_write(uint8_t RegisterAddress, uint8_t *pData, uint8_t Length)
{
	int tmo;

	if (Length == 0 || pData == 0)
		return;  
  
	while (UCB1STAT & UCBBUSY)
		nop();
  
	AccelerometerBusy = 1;
	LengthCount = Length;
	Index = 0;
	pAccelerometerData = pData;
  
	/* 
	 * enable transmit interrupt and 
	 * setup for write and send the start condition
	 */
	ACCELEROMETER_IFG = 0;
	ACCELEROMETER_CTL1 |= UCTR + UCTXSTT;
	while(!(ACCELEROMETER_IFG & UCTXIFG))
		nop();
  
	/* 
	 * clear transmit interrupt flag, enable interrupt, 
	 * send the register address
	 */
	ACCELEROMETER_IFG = 0;
	ACCELEROMETER_IE |= UCTXIE;
	ACCELEROMETER_TXBUF = RegisterAddress;

	tmo = 0;
	while (AccelerometerBusy) {
		while (tmo++ < 1000)
			__delay_cycles(16000);
		if (tmo >= 1000) {
			debug_uart_tx("ACC I2C tx tmo\n");
			return;
		}
	}

	while (ACCELEROMETER_CTL1 & UCTXSTP)
		nop();

	/* the rest of TX will be handled by the ISR */
}

void mw_acc_i2c_read_single(const uint8_t RegisterAddress, const uint8_t *pData)
{
	if ( pData == 0 )
		return;
  
	/* wait for bus to be free */
	while (UCB1STAT & UCBBUSY)
		nop();
  
	AccelerometerBusy = 1;
	LengthCount = 1;
	Index = 0;
	pAccelerometerData = (uint8_t *)pData;

	/* transmit address */
	ACCELEROMETER_IFG = 0;
	ACCELEROMETER_CTL1 |= UCTR + UCTXSTT;
	while (!(ACCELEROMETER_IFG & UCTXIFG))
		nop();
  
	/* write register address */
	ACCELEROMETER_IFG = 0;
	ACCELEROMETER_TXBUF = RegisterAddress;
	while (!(ACCELEROMETER_IFG & UCTXIFG))
		nop();

	/* send a repeated start (same slave address now it is a read command) 
	 * read possible extra character from rxbuffer
	 */
	ACCELEROMETER_RXBUF;
	ACCELEROMETER_IFG = 0;
	ACCELEROMETER_IE |= UCRXIE;
	ACCELEROMETER_CTL1 &= ~UCTR;
	/* for a read of a single byte the stop must be sent while the byte is being 
	 * received. If this is interrupted an extra byte may be read.
	 * however, it will be discarded during the next read
	 */
	if (LengthCount == 1) {
		/* errata usci30: prevent interruption of sending stop 
		 * so that only one byte is read
		 * this requires 62 us @ 320 kHz, 51 @ 400 kHz
		 */
		__disable_interrupt();

		ACCELEROMETER_CTL1 |= UCTXSTT;
  
		while(ACCELEROMETER_CTL1 & UCTXSTT)
			nop();

		ACCELEROMETER_CTL1 |= UCTXSTP;

		__enable_interrupt();
	} else {
		ACCELEROMETER_CTL1 |= UCTXSTT;
	}
  
	/* wait until all data has been received and the stop bit has been sent */
	while (AccelerometerBusy)
		nop();
	while (ACCELEROMETER_CTL1 & UCTXSTP)
		nop();
	Index = 0;
	pAccelerometerData = 0;
}

/* errata usci30: only perform single reads 
 * second solution: use DMA
 */
void mw_acc_i2c_read(const uint8_t RegisterAddress, uint8_t *pData, const uint8_t Length)
{
	int i;

	for ( i = 0; i < Length; i++ ) {
		mw_acc_i2c_read_single(RegisterAddress + i, (pData + i));
	}
}


void mw_acc_init(void)
{
	uint8_t WriteRegisterData;
	uint8_t pReadRegisterData[4];
#if defined MW_DEVBOARD_V2
	char tstr[16];
#endif

	// it takes at least 20ms to power up
	ENABLE_ACCELEROMETER_POWER();
	__delay_cycles(320000);

	mw_acc_init_i2c();

	/*
	 * make sure part is in standby mode because some registers can only
	 * be changed when the part is not active.
	 */
	WriteRegisterData = PC1_STANDBY_MODE;
	mw_acc_i2c_write(KIONIX_CTRL_REG1, &WriteRegisterData, 1);

	/* enable face-up and face-down detection */
	WriteRegisterData = TILT_FDM | TILT_FUM;
	mw_acc_i2c_write(KIONIX_CTRL_REG2, &WriteRegisterData, 1);
    
	/* 
	 * the interrupt from the accelerometer can be used to get periodic data
	 * the real time clock can also be used
	 */
  
	/* change to output data rate to 25 Hz */
	WriteRegisterData = WUF_ODR_25HZ | TAP_ODR_400HZ;
	mw_acc_i2c_write(KIONIX_CTRL_REG3, &WriteRegisterData, 1);
  
	/* enable interrupt and make it active high */
	WriteRegisterData = IEN | IEA;
	mw_acc_i2c_write(KIONIX_INT_CTRL_REG1, &WriteRegisterData, 1);
  
	/* enable motion detection interrupt for all three axis */
	WriteRegisterData = XBW | YBW | ZBW;
	mw_acc_i2c_write(KIONIX_INT_CTRL_REG2, &WriteRegisterData, 1);

	/* enable tap interrupt for Z-axis */
	WriteRegisterData = TFDM;
	mw_acc_i2c_write(KIONIX_INT_CTRL_REG3, &WriteRegisterData, 1);
  
	/* set TDT_TIMER to 0.2 secs*/
	WriteRegisterData = 0x50;
	mw_acc_i2c_write(KIONIX_TDT_TIMER, &WriteRegisterData, 1);
  
	/* set tap low and high thresholds (default: 26 and 182) */
	WriteRegisterData = 40; //78;
	mw_acc_i2c_write(KIONIX_TDT_L_THRESH, &WriteRegisterData, 1);
	WriteRegisterData = 128;
	mw_acc_i2c_write(KIONIX_TDT_H_THRESH, &WriteRegisterData, 1);
    
	/* set WUF_TIMER counter */
	WriteRegisterData = 10;
	mw_acc_i2c_write(KIONIX_WUF_TIMER, &WriteRegisterData, 1);
    
	/* this causes data to always be sent */
	// WriteRegisterData = 0x00;
	WriteRegisterData = 0x01 /*0x08*/;
	mw_acc_i2c_write(KIONIX_WUF_THRESH, &WriteRegisterData, 1);
     
	/* single byte read test */
	mw_acc_i2c_read(KIONIX_DCST_RESP, pReadRegisterData, 1);
#if defined MW_DEVBOARD_V2
	snprintf(tstr, 16, "acc DCST 0x%02x\n", pReadRegisterData[0]);
	debug_uart_tx(tstr);
#endif
  
	/* multiple byte read test */
	mw_acc_i2c_read(KIONIX_WHO_AM_I, pReadRegisterData, 2);
#if defined MW_DEVBOARD_V2
	snprintf(tstr, 16, "acc is 0x%02x 0x%02x\n", pReadRegisterData[0], pReadRegisterData[1]);
	debug_uart_tx(tstr);
#endif

	/* 
	 * KIONIX_CTRL_REG3 and DATA_CTRL_REG can remain at their default values 
	 *
	 * 50 Hz
	 */
#if 0  
	/* KTXF9 300 uA; KTXI9 165 uA */
	WriteRegisterData = PC1_OPERATING_MODE | TAP_ENABLE_TDTE;
  
	/* 180 uA; KTXI9 115 uA */
	WriteRegisterData = PC1_OPERATING_MODE | RESOLUTION_8BIT | WUF_ENABLE;

	/* 180 uA; KTXI9 8.7 uA */
	WriteRegisterData = PC1_OPERATING_MODE | TILT_ENABLE_TPE;

	/* 720 uA; KTXI9 330 uA */  
	WriteRegisterData = PC1_OPERATING_MODE | RESOLUTION_12BIT | WUF_ENABLE;
#endif
  
	/* setup the default for the AccelerometerEnable command */
#if 0
	OperatingModeRegister = PC1_OPERATING_MODE | RESOLUTION_12BIT | TAP_ENABLE_TDTE | TILT_ENABLE_TPE; // | WUF_ENABLE;
	InterruptControl = INTERRUPT_CONTROL_DISABLE_INTERRUPT;
	SidControl = SID_CONTROL_SEND_DATA;
	SidAddr = KIONIX_XOUT_L;
	SidLength = XYZ_DATA_LENGTH;  

	AccelState = ACCEL_STATE_INIT;
#endif
}

void mw_acc_enable(void)
{
	uint8_t sdata;

	mw_acc_init();

	sdata = PC1_OPERATING_MODE | RESOLUTION_12BIT | TAP_ENABLE_TDTE | TILT_ENABLE_TPE | WUF_ENABLE;
	//sdata = PC1_OPERATING_MODE | RESOLUTION_8BIT | TAP_ENABLE_TDTE | TILT_ENABLE_TPE; // | WUF_ENABLE;
	mw_acc_i2c_write(KIONIX_CTRL_REG1, &sdata, 1);
  
	ACCELEROMETER_INT_ENABLE();
	mw_acc_i2c_read(KIONIX_INT_REL, &sdata, 1);
	AccelState = ACCEL_STATE_ENABLED;
}

void mw_acc_disable(void)
{
	uint8_t sdata;

	if (AccelState == ACCEL_STATE_ENABLED) {
		sdata = PC1_STANDBY_MODE;
		mw_acc_i2c_write(KIONIX_CTRL_REG1, &sdata, 1);

		ACCELEROMETER_INT_DISABLE();
		mw_acc_disable_i2c();
		/// DISABLE_ACCELEROMETER_POWER();
		AccelState = ACCEL_STATE_DISABLED;
	}
}

void mw_acc_read(int16_t *x, int16_t *y, int16_t *z)
{
	uint8_t rdata[6];

	if (AccelState == ACCEL_STATE_ENABLED) {
		mw_acc_i2c_read(KIONIX_XOUT_L, rdata, 6);

		*x = rdata[0] | (rdata[1] << 8);
		*y = rdata[2] | (rdata[3] << 8);
		*z = rdata[4] | (rdata[5] << 8);
	} else {
		*x = 0;
		*y = 0;
		*z = 0;
	}
}

void mw_acc_handle_irq(void)
{
	uint8_t sdata, srcreg1, srcreg2;
#if defined MW_DEVBOARD_V2
	char tstr[16];
#endif

	if (AccelState != ACCEL_STATE_ENABLED)
		return;

	mw_acc_i2c_read(KIONIX_INT_SRC_REG1, &srcreg1, 1);
#if defined MW_DEVBOARD_V2
	snprintf(tstr, 16, "accsrc1: 0x%02x\n", srcreg1);
	debug_uart_tx(tstr);
#endif
	mw_acc_i2c_read(KIONIX_INT_SRC_REG2, &srcreg2, 1);
#if defined MW_DEVBOARD_V2
	snprintf(tstr, 16, "accsrc2: 0x%02x\n", srcreg2);
	debug_uart_tx(tstr);
#endif
	if (srcreg1 & INT_TAP_SINGLE) {
	};
	if (srcreg1 & INT_TAP_DOUBLE) {
	};
	if (srcreg2 & INT_WUFS) {
		int16_t x, y, z;
		mw_acc_read(&x, &y, &z);
		oswald_handle_accel_event((int8_t)(x / (32768 / 255)), (int8_t)(y / (32768 / 255)), (int8_t)(z / (32768 / 255)));
	}

	mw_acc_i2c_read(KIONIX_INT_REL, &sdata, 1);
}