/*---------------------------------------------------------------------------------
	DS Motion Card/DS Motion Pak functionality

	Copyright (C) 2007
		Michael Noland (joat)
		Jason Rogers (dovoto)
		Dave Murphy (WinterMute)
		Keith Epstein (KeithE)

	This software is provided 'as-is', without any express or implied
	warranty.  In no event will the authors be held liable for any
	damages arising from the use of this software.

	Permission is granted to anyone to use this software for any
	purpose, including commercial applications, and to alter it and
	redistribute it freely, subject to the following restrictions:

	1.	The origin of this software must not be misrepresented; you
		must not claim that you wrote the original software. If you use
		this software in a product, an acknowledgment in the product
		documentation would be appreciated but is not required.
	2.	Altered source versions must be plainly marked as such, and
		must not be misrepresented as being the original software.
	3.	This notice may not be removed or altered from any source
		distribution.

---------------------------------------------------------------------------------*/

#include <nds/card.h>
#include <nds/system.h>
#include <nds/memory.h>
#include <nds/bios.h>
#include <nds/arm9/ndsmotion.h>

#define WAIT_CYCLES 185

#define CARD_WaitBusy()   while (CARD_CR1 & /*BUSY*/0x80);

// enables SPI bus at 4.19 MHz
#define SPI_On() CARD_CR1 = /*E*/0x8000 | /*SEL*/0x2000 | /*MODE*/0x40 | 0;

// disables SPI bus
#define SPI_Off() CARD_CR1 = 0;

// Volatile GBA bus SRAM for reading from DS Motion Pak
#define V_SRAM ((volatile unsigned char*)0x0A000000)


int card_type = -1;

//these are the default calibration values for sensitivity and offset
MotionCalibration calibration = {2048, 2048, 2048, 1680, 819, 819, 819, 825};

// sends and receives 1 byte on the SPI bus
unsigned char motion_spi(unsigned char in_byte){

	unsigned char out_byte;
	CARD_EEPDATA = in_byte; // send the output byte to the SPI bus
	CARD_WaitBusy(); // wait for transmission to complete
	out_byte=CARD_EEPDATA; // read the input byte from the SPI bus
	return out_byte;
}


void motion_MK6_sensor_mode(void) {
	// send some commands on the SPI bus
	SPI_On()
	motion_spi(0xFE);
	SPI_Off()
	SPI_On()
	motion_spi(0xFD);
	SPI_Off()
	SPI_On()
	motion_spi(0xFB);
	SPI_Off()
	SPI_On()
	motion_spi(0xF8);
	SPI_Off()
}

void motion_MK6_EEPROM_mode(void) {
	// send some commands on the SPI bus
	SPI_On()
	motion_spi(0xFE);
	SPI_Off()
	SPI_On()
	motion_spi(0xFD);
	SPI_Off()
	SPI_On()
	motion_spi(0xFB);
	SPI_Off()
	SPI_On()
	motion_spi(0xF9);
	SPI_Off()
}

// checks whether a DS Motion Pak is plugged in
int motion_pak_is_inserted(void){
    int motion_pak = 0;
	unsigned char return_byte = V_SRAM[10]; // read first byte of DS Motion Pak check
	swiDelay(WAIT_CYCLES);
	return_byte = V_SRAM[0];
	swiDelay(WAIT_CYCLES);
	if (return_byte==0xF0) { // DS Motion Pak returns 0xF0
		return_byte = V_SRAM[0]; // read second byte of DS Motion Pak check
		swiDelay(WAIT_CYCLES);
		if(return_byte==0x0F) { // DS Motion Pak returns 0x0F
			motion_pak = 1;
		}
	}
    return motion_pak;
}

// checks whether a DS Motion Card is plugged in
// this only works after motion_init()
// it will return false if it is run before motion_init()
int motion_card_is_inserted(void){
	// send 0x03 to read from DS Motion Card control register
	SPI_On()
	motion_spi(0x03); // command to read from control register
	// if the control register is 0x04 then the enable was successful
	if( motion_spi(0x00) == 0x04)
	{
		SPI_Off()
		return 1;
	}
	SPI_Off();
	return 0;
}

// turn on the DS Motion Sensor (DS Motion Pak or DS Motion Card)
// Requires knowing which type is present (can be found by using motion_init)
int motion_enable(int card_type) {
	switch (card_type)
	{
		case 1: // DS Motion Pak - automatically enabled on powerup
			// check to see whether Motion Pak is alive
			return motion_pak_is_inserted();
			break;
		case 2: // DS Motion Card
			// send 0x04, 0x04 to enable
			SPI_On()
			motion_spi(0x04); // command to write to control register
			motion_spi(0x04); // enable
			SPI_Off()
			// check to see whether Motion Card is alive
			return motion_card_is_inserted();
			break;
		case 3: // MK6 - same command as DS Motion Card
			// send 0x04, 0x04 to enable
			SPI_On()
			motion_spi(0x04); // command to write to control register
			motion_spi(0x04); // enable
			SPI_Off()
			// check to see whether Motion Card is alive
			return motion_card_is_inserted();
			break;
		default: // if input parameter is not recognized, return 0
			return 0;
			break;
	}
}

// Initialize the DS Motion Sensor
// Determines which DS Motion Sensor is present
// Turns it on
// Does not require knowing which type is present
int motion_init(void) {
	sysSetBusOwners(true, true);
	// first, check for the DS Motion Pak - type 1
	if( motion_pak_is_inserted() == 1 )
    {
        card_type = 1;
        return 1;
	}// next, check for DS Motion Card - type 2
	if( motion_enable(2) == 1 )
    {
         card_type = 2;
         return 2;
	}

    motion_MK6_sensor_mode(); // send command to switch MK6 to sensor mode

    if( motion_enable(3) == 1 )
    {
        card_type = 3;
        return 3;
	}// if neither cases are true, then return 0 to indicate no DS Motion Sensor
	return 0;
}

// Deinitialize the DS Motion Sensor
// In the case of a DS Motion Pak, do nothing - there is nothing to de-init
// In the case of a DS Motion Card, turns off the accelerometer
// In the case of an MK6, turns off accelerometer and switches out of sensor mode into EEPROM mode
void motion_deinit(void) {
	// DS Motion Card - turn off accelerometer
	SPI_On()
	motion_spi(0x04); // command to write to control register
	motion_spi(0x00); // turn it off
	SPI_Off()
	// MK6 - switch to EEPROM mode
	motion_MK6_EEPROM_mode(); // switch MK6 to EEPROM mode
}

// read the X acceleration
signed int motion_read_x(void) {
	unsigned char High_byte = 0;
	unsigned char Low_byte = 0;
	signed int output = 0;
	switch(card_type)
	{
		case 1: // DS Motion Pak
			High_byte = V_SRAM[2]; // Command to load X High onto bus
			swiDelay(WAIT_CYCLES); // wait for data ready
			High_byte = V_SRAM[0]; // get the high byte
			swiDelay(WAIT_CYCLES); // wait for data ready
			Low_byte = V_SRAM[0]; // get the low byte
			swiDelay(WAIT_CYCLES); // wait after for Motion Pak to be ready for next command
			output = (signed int)( (High_byte<<8 | Low_byte)>>4);
			return output;
			break;
		case 2: // DS Motion Card
			SPI_On()
			motion_spi(0x00); // command to convert X axis
			swiDelay(625); // wait at least 40 microseconds for the A-D conversion
			output = ( (motion_spi(0x00)<<8)|motion_spi(0x00) )>>4; // read 16 bits and store as a 12 bit number
			SPI_Off()
			return output;
			break;
		case 3: // MK6 - same command as DS Motion Card
			SPI_On()
			motion_spi(0x00); // command to convert X axis
			swiDelay(625); // wait at least 40 microseconds for the A-D conversion
			output = ( (motion_spi(0x00)<<8)|motion_spi(0x00) )>>4; // read 16 bits and store as a 12 bit number
			SPI_Off()
			return output;
			break;
		default:
			return 0;
			break;
	}
}

// read the Y acceleration
signed int motion_read_y(void) {
	unsigned char High_byte = 0;
	unsigned char Low_byte = 0;
	signed int output = 0;
	switch (card_type)
	{
		case 1: // DS Motion Pak
			High_byte = V_SRAM[4]; // Command to load Y High onto bus
			swiDelay(WAIT_CYCLES); // wait for data ready
			High_byte = V_SRAM[0]; // get the high byte
			swiDelay(WAIT_CYCLES); // wait for data ready
			Low_byte = V_SRAM[0]; // get the low byte
			swiDelay(WAIT_CYCLES); // wait after for Motion Pak to be ready for next command
			output = (signed int)( (High_byte<<8 | Low_byte)>>4);
			return output;
			break;
		case 2: // DS Motion Card
			SPI_On()
			motion_spi(0x02); // command to convert Y axis
			swiDelay(625); // wait at least 40 microseconds for the A-D conversion
			output = ( (motion_spi(0x00)<<8)|motion_spi(0x00) )>>4; // read 16 bits and store as a 12 bit number
			SPI_Off()
			return output;
			break;
		case 3: // MK6 - same command as DS Motion Card
			SPI_On()
			motion_spi(0x02); // command to convert Y axis
			swiDelay(625); // wait at least 40 microseconds for the A-D conversion
			output = ( (motion_spi(0x00)<<8)|motion_spi(0x00) )>>4; // read 16 bits and store as a 12 bit number
			SPI_Off()
			return output;
			break;
		default:
			return 0;
			break;
	}
}

// read the Z acceleration
signed int motion_read_z(void) {
	unsigned char High_byte = 0;
	unsigned char Low_byte = 0;
	signed int output = 0;
	switch (card_type)
	{
		case 1: // DS Motion Pak
			High_byte = V_SRAM[6]; // Command to load Z High onto bus
			swiDelay(WAIT_CYCLES); // wait for data ready
			High_byte = V_SRAM[0]; // get the high byte
			swiDelay(WAIT_CYCLES); // wait for data ready
			Low_byte = V_SRAM[0]; // get the low byte
			swiDelay(WAIT_CYCLES); // wait after for Motion Pak to be ready for next command
			output = (signed int)( (High_byte<<8 | Low_byte)>>4);
			return output;
			break;
		case 2: // DS Motion Card
			SPI_On()
			motion_spi(0x01); // command to convert Z axis
			swiDelay(625); // wait at least 40 microseconds for the A-D conversion
			output = ( (motion_spi(0x00)<<8)|motion_spi(0x00) )>>4; // read 16 bits and store as a 12 bit number
			SPI_Off()
			return output;
			break;
		case 3: // MK6 - same command as DS Motion Card
			SPI_On()
			motion_spi(0x01); // command to convert Z axis
			swiDelay(625); // wait at least 40 microseconds for the A-D conversion
			output = ( (motion_spi(0x00)<<8)|motion_spi(0x00) )>>4; // read 16 bits and store as a 12 bit number
			SPI_Off()
			return output;
			break;
		default:
			return 0;
			break;
	}
}

// read the Z rotation (gyro)
signed int motion_read_gyro(void) {
	unsigned char High_byte = 0;
	unsigned char Low_byte = 0;
	signed int output = 0;
	switch (card_type)
	{
		case 1: // DS Motion Pak
			High_byte = V_SRAM[8]; // Command to load Gyro High onto bus
			swiDelay(WAIT_CYCLES); // wait for data ready
			High_byte = V_SRAM[0]; // get the high byte
			swiDelay(WAIT_CYCLES); // wait for data ready
			Low_byte = V_SRAM[0]; // get the low byte
			swiDelay(WAIT_CYCLES); // wait after for Motion Pak to be ready for next command
			output = (signed int)( (High_byte<<8 | Low_byte)>>4);
			return output;
			break;
		case 2: // DS Motion Card
			SPI_On()
			motion_spi(0x07); // command to convert Gyro axis
			swiDelay(625); // wait at least 40 microseconds for the A-D conversion
			output = ( (motion_spi(0x00)<<8)|motion_spi(0x00) )>>4; // read 16 bits and store as a 12 bit number
			SPI_Off()
			return output;
			break;
		case 3: // MK6 - same command as DS Motion Card
			SPI_On()
			motion_spi(0x07); // command to convert Gyro axis
			swiDelay(625); // wait at least 40 microseconds for the A-D conversion
			output = ( (motion_spi(0x00)<<8)|motion_spi(0x00) )>>4; // read 16 bits and store as a 12 bit number
			SPI_Off()
			return output;
			break;
		default:
			return 0;
			break;
	}
}

//gets acceleration value in mili G (where g is 9.8 m/s*s)
int motion_acceleration_x(void){
	int accel = motion_read_x();
	return (accel - calibration.xoff) * 1000 / calibration.xsens;
}

//gets acceleration value in mili G (where g is 9.8 m/s*s)
int motion_acceleration_y(void){
	int accel = motion_read_y();
	return (accel - calibration.yoff) * 1000 / calibration.ysens;
}
//gets acceleration value in mili G (where g is 9.8 m/s*s)
int motion_acceleration_z(void){
	int accel = motion_read_z();
	return (accel - calibration.zoff) * 1000 / calibration.zsens;
}

//converts raw rotation value to degrees per second
int motion_rotation(void){
	int rotation = motion_read_gyro();
	return (rotation - calibration.goff) * 1000 / calibration.gsens;
}

//this should be passed the raw reading at 1g for accurate
//acceleration calculations.  Default is 819
void motion_set_sens_x(int sens){
	calibration.xsens = sens - calibration.xoff;
}

//this should be passed the raw reading at 1g for accurate
//acceleration calculations.  Default is 819
void motion_set_sens_y(int sens){
	calibration.ysens = sens - calibration.yoff;
}

//this should be passed the raw reading at 1g for accurate
//acceleration calculations.  Default is 819
void motion_set_sens_z(int sens){
	calibration.zsens = sens - calibration.zoff;
}

//this should be passed the raw reading at 1g for accurate
//acceleration calculations.  Default is 825
void motion_set_sens_gyro(int sens){
	calibration.gsens = sens;
}

//this should be called when the axis is under no acceleration
//default is 2048
void motion_set_offs_x(void){
	calibration.xoff = motion_read_x();
}

//this should be called when the axis is under no acceleration
//default is 2048
void motion_set_offs_y(void){
	calibration.yoff = motion_read_y();
}

//this should be called when the axis is under no acceleration
//default is 2048
void motion_set_offs_z(void){
	calibration.zoff = motion_read_z();
}

//this should be called when the axis is under no acceleration
//default is 1680
void motion_set_offs_gyro(void){
	calibration.goff = motion_read_gyro();
}

MotionCalibration* motion_get_calibration(void){
	return &calibration;
}

void motion_set_calibration(MotionCalibration* cal){
	calibration.xsens = cal->xsens;
	calibration.ysens = cal->ysens;
	calibration.zsens = cal->zsens;
	calibration.gsens = cal->gsens;
	calibration.xoff = cal->xoff;
	calibration.yoff = cal->yoff;
	calibration.zoff = cal->zoff;
	calibration.goff = cal->goff;
}

// enable analog input number 1 (ain_1)
void motion_enable_ain_1(void){
	unsigned char return_byte;
    return_byte = V_SRAM[16];
	swiDelay(WAIT_CYCLES);
}

// enable analog input number 2 (ain_2)
void motion_enable_ain_2(void){
	unsigned char return_byte;
    return_byte = V_SRAM[18];
	swiDelay(WAIT_CYCLES);
}

// read from the analog input number 1 - requires enabling ain_1 first
int motion_read_ain_1(void){
	unsigned char High_byte = V_SRAM[12]; // Command to load AIN_1 High onto bus
	swiDelay(WAIT_CYCLES); // wait for data ready
	High_byte = V_SRAM[0]; // get the high byte
	swiDelay(WAIT_CYCLES); // wait for data ready
	unsigned char Low_byte = V_SRAM[0]; // get the low byte
	swiDelay(WAIT_CYCLES); // wait after for Motion Pak to be ready for next command
	signed int output = (signed int)( (High_byte<<8 | Low_byte)>>4);
	return output;
}

// read from the analog input number 2 - requires enabling ain_2 first
int motion_read_ain_2(void){
	unsigned char High_byte = V_SRAM[14]; // Command to load AIN_1 High onto bus
	swiDelay(WAIT_CYCLES); // wait for data ready
	High_byte = V_SRAM[0]; // get the high byte
	swiDelay(WAIT_CYCLES); // wait for data ready
	unsigned char Low_byte = V_SRAM[0]; // get the low byte
	swiDelay(WAIT_CYCLES); // wait after for Motion Pak to be ready for next command
	signed int output = (signed int)( (High_byte<<8 | Low_byte)>>4);
	return output;
}