/******************************************************************************
 * $Id: AKFS_APIs.c 580 2012-03-29 09:56:21Z yamada.rj $
 ******************************************************************************
 *
 * Copyright (C) 2012 Asahi Kasei Microdevices Corporation, Japan
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include "AKFS_Common.h"
#include "AKFS_Disp.h"
#include "AKFS_FileIO.h"
#include "AKFS_APIs.h"

#ifdef WIN32
#include "AK8975_LinuxDriver.h"
#endif

static AK8975PRMS g_prms;

/*!
  Initialize library. At first, 0 is set to all parameters.  After that, some
  parameters, which should not be 0, are set to specific value. Some of initial
  values can be customized by editing the file \c "AKFS_CSpec.h".
  @return The return value is #AKM_SUCCESS.
  @param[in] hpat Specify a layout pattern number.  The number is determined
  according to the mount orientation of the magnetometer.
  @param[in] regs[3] Specify the ASA values which are read out from
  fuse ROM.  regs[0] is ASAX, regs[1] is ASAY, regs[2] is ASAZ.
 */
int16 AKFS_Init(
	const	AKFS_PATNO	hpat,
	const	uint8		regs[]
)
{
	AKMDATA(AKMDATA_DUMP, "%s: hpat=%d, r[0]=0x%02X, r[1]=0x%02X, r[2]=0x%02X\n",
		__FUNCTION__, hpat, regs[0], regs[1], regs[2]);

	/* Set 0 to the AK8975 structure. */
	memset(&g_prms, 0, sizeof(AK8975PRMS));

	/* Sensitivity */
	g_prms.mfv_hs.u.x = AK8975_HSENSE_DEFAULT;
	g_prms.mfv_hs.u.y = AK8975_HSENSE_DEFAULT;
	g_prms.mfv_hs.u.z = AK8975_HSENSE_DEFAULT;
	g_prms.mfv_as.u.x = AK8975_ASENSE_DEFAULT;
	g_prms.mfv_as.u.y = AK8975_ASENSE_DEFAULT;
	g_prms.mfv_as.u.z = AK8975_ASENSE_DEFAULT;

	/* Initialize variables that initial value is not 0. */
	g_prms.mi_hnaveV = CSPEC_HNAVE_V;
	g_prms.mi_hnaveD = CSPEC_HNAVE_D;
	g_prms.mi_anaveV = CSPEC_ANAVE_V;
	g_prms.mi_anaveD = CSPEC_ANAVE_D;

	/* Copy ASA values */
	g_prms.mi_asa.u.x = regs[0];
	g_prms.mi_asa.u.y = regs[1];
	g_prms.mi_asa.u.z = regs[2];

	/* Copy layout pattern */
	g_prms.m_hpat = hpat;

	return AKM_SUCCESS;
}

/*!
  Release resources. This function is for future expansion.
  @return The return value is #AKM_SUCCESS.
 */
int16 AKFS_Release(void)
{
	return AKM_SUCCESS;
}

/*
  This function is called just before a measurement sequence starts.
  This function reads parameters from file, then initializes algorithm
  parameters.
  @return The return value is #AKM_SUCCESS.
  @param[in] path Specify a path to the settings file.
 */
int16 AKFS_Start(
	const char* path
)
{
	AKMDATA(AKMDATA_DUMP, "%s: path=%s\n", __FUNCTION__, path);

	/* Read setting files from a file */
	if (AKFS_LoadParameters(&g_prms, path) != AKM_SUCCESS) {
		AKMERROR_STR("AKFS_Load");
	}

	/* Initialize buffer */
	AKFS_InitBuffer(AKFS_HDATA_SIZE, g_prms.mfv_hdata);
	AKFS_InitBuffer(AKFS_HDATA_SIZE, g_prms.mfv_hvbuf);
	AKFS_InitBuffer(AKFS_ADATA_SIZE, g_prms.mfv_adata);
	AKFS_InitBuffer(AKFS_ADATA_SIZE, g_prms.mfv_avbuf);

	/* Initialize for AOC */
	AKFS_InitAOC(&g_prms.m_aocv);
	/* Initialize magnetic status */
	g_prms.mi_hstatus = 0;

	return AKM_SUCCESS;
}

/*!
  This function is called when a measurement sequence is done.
  This fucntion writes parameters to file.
  @return The return value is #AKM_SUCCESS.
  @param[in] path Specify a path to the settings file.
 */
int16 AKFS_Stop(
	const char* path
)
{
	AKMDATA(AKMDATA_DUMP, "%s: path=%s\n", __FUNCTION__, path);

	/* Write setting files to a file */
	if (AKFS_SaveParameters(&g_prms, path) != AKM_SUCCESS) {
		AKMERROR_STR("AKFS_Save");
	}

	return AKM_SUCCESS;
}

/*!
  This function is called when new magnetometer data is available.  The output
  vector format and coordination system follow the Android definition.
  @return The return value is #AKM_SUCCESS.
   Otherwise the return value is #AKM_FAIL.
  @param[in] mag A set of measurement data from magnetometer.  X axis value
   should be in mag[0], Y axis value should be in mag[1], Z axis value should be 
   in mag[2].
  @param[in] status A status of magnetometer.  This status indicates the result
   of measurement data, i.e. overflow, success or fail, etc.
  @param[out] vx X axis value of magnetic field vector.
  @param[out] vy Y axis value of magnetic field vector.
  @param[out] vz Z axis value of magnetic field vector.
  @param[out] accuracy Accuracy of magnetic field vector.
 */
int16 AKFS_Get_MAGNETIC_FIELD(
	const	int16		mag[3],
	const	int16		status,
			AKFLOAT*	vx,
			AKFLOAT*	vy,
			AKFLOAT*	vz,
			int16*		accuracy
)
{
	int16 akret;
	int16 aocret;
	AKFLOAT radius;

	AKMDATA(AKMDATA_DUMP, "%s: m[0]=%d, m[1]=%d, m[2]=%d, st=%d\n",
		__FUNCTION__, mag[0], mag[1], mag[2], status);

	/* Decomposition */
	/* Sensitivity adjustment, i.e. multiply ASA, is done in this function. */
	akret = AKFS_DecompAK8975(
		mag,
		status,
		&g_prms.mi_asa,
		AKFS_HDATA_SIZE,
		g_prms.mfv_hdata
	);
	if(akret == AKFS_ERROR) {
		AKMERROR;
		return AKM_FAIL;
	}

	/* Adjust coordination */
	akret = AKFS_Rotate(
		g_prms.m_hpat,
		&g_prms.mfv_hdata[0]
	);
	if (akret == AKFS_ERROR) {
		AKMERROR;
		return AKM_FAIL;
	}

	/* AOC for magnetometer */
	/* Offset estimation is done in this function */
	aocret = AKFS_AOC(
		&g_prms.m_aocv,
		g_prms.mfv_hdata,
		&g_prms.mfv_ho
	);

	/* Subtract offset */
	/* Then, a magnetic vector, the unit is uT, is stored in mfv_hvbuf. */
	akret = AKFS_VbNorm(
		AKFS_HDATA_SIZE,
		g_prms.mfv_hdata,
		1,
		&g_prms.mfv_ho,
		&g_prms.mfv_hs,
		AK8975_HSENSE_TARGET,
		AKFS_HDATA_SIZE,
		g_prms.mfv_hvbuf
	);
	if(akret == AKFS_ERROR) {
		AKMERROR;
		return AKM_FAIL;
	}

	/* Averaging */
	akret = AKFS_VbAve(
		AKFS_HDATA_SIZE,
		g_prms.mfv_hvbuf,
		CSPEC_HNAVE_V,
		&g_prms.mfv_hvec
	);
	if (akret == AKFS_ERROR) {
		AKMERROR;
		return AKM_FAIL;
	}

	/* Check the size of magnetic vector */
	radius = AKFS_SQRT(
			(g_prms.mfv_hvec.u.x * g_prms.mfv_hvec.u.x) +
			(g_prms.mfv_hvec.u.y * g_prms.mfv_hvec.u.y) +
			(g_prms.mfv_hvec.u.z * g_prms.mfv_hvec.u.z));

	if (radius > AKFS_GEOMAG_MAX) {
		g_prms.mi_hstatus = 0;
	} else {
		if (aocret) {
			g_prms.mi_hstatus = 3;
		}
	}

	*vx = g_prms.mfv_hvec.u.x;
	*vy = g_prms.mfv_hvec.u.y;
	*vz = g_prms.mfv_hvec.u.z;
	*accuracy = g_prms.mi_hstatus;

	/* Debug output */
	AKMDATA(AKMDATA_MAG, "Mag(%d):%8.2f, %8.2f, %8.2f\n",
			*accuracy, *vx, *vy, *vz);

	return AKM_SUCCESS;
}

/*!
  This function is called when new accelerometer data is available.  The output
  vector format and coordination system follow the Android definition.
  @return The return value is #AKM_SUCCESS when function succeeds. Otherwise
   the return value is #AKM_FAIL.
  @param[in] acc A set of measurement data from accelerometer.  X axis value
   should be in acc[0], Y axis value should be in acc[1], Z axis value should be 
   in acc[2].
  @param[in] status A status of accelerometer.  This status indicates the result
   of acceleration data, i.e. overflow, success or fail, etc.
  @param[out] vx X axis value of acceleration vector.
  @param[out] vy Y axis value of acceleration vector.
  @param[out] vz Z axis value of acceleration vector.
  @param[out] accuracy Accuracy of acceleration vector.
  This value is always 3.
 */
int16 AKFS_Get_ACCELEROMETER(
	const   int16		acc[3],
	const	int16		status,
			AKFLOAT*	vx,
			AKFLOAT*	vy,
			AKFLOAT*	vz,
			int16*		accuracy
)
{
	int16 akret;

	AKMDATA(AKMDATA_DUMP, "%s: a[0]=%d, a[1]=%d, a[2]=%d, st=%d\n",
		__FUNCTION__, acc[0], acc[1], acc[2], status);

	/* Save data to buffer */
	AKFS_BufShift(
		AKFS_ADATA_SIZE,
		1,
		g_prms.mfv_adata
	);
	g_prms.mfv_adata[0].u.x = acc[0];
	g_prms.mfv_adata[0].u.y = acc[1];
	g_prms.mfv_adata[0].u.z = acc[2];

	/* Subtract offset, adjust sensitivity */
	/* As a result, a unit of acceleration data in mfv_avbuf is '1G = 9.8' */
	akret = AKFS_VbNorm(
		AKFS_ADATA_SIZE,
		g_prms.mfv_adata,
		1,
		&g_prms.mfv_ao,
		&g_prms.mfv_as,
		AK8975_ASENSE_TARGET,
		AKFS_ADATA_SIZE,
		g_prms.mfv_avbuf
	);
	if(akret == AKFS_ERROR) {
		AKMERROR;
		return AKM_FAIL;
	}

	/* Averaging */
	akret = AKFS_VbAve(
		AKFS_ADATA_SIZE,
		g_prms.mfv_avbuf,
		CSPEC_ANAVE_V,
		&g_prms.mfv_avec
	);
	if (akret == AKFS_ERROR) {
		AKMERROR;
		return AKM_FAIL;
	}

	/* Adjust coordination */
	/* It is not needed. Because, the data from AK8975 driver is already
	   follows Android coordinate system. */

	*vx = g_prms.mfv_avec.u.x;
	*vy = g_prms.mfv_avec.u.y;
	*vz = g_prms.mfv_avec.u.z;
	*accuracy = 3;

	/* Debug output */
	AKMDATA(AKMDATA_ACC, "Acc(%d):%8.2f, %8.2f, %8.2f\n",
			*accuracy, *vx, *vy, *vz);

	return AKM_SUCCESS;
}

/*!
  Get orientation sensor's elements. The vector format and coordination system
   follow the Android definition.  Before this function is called, magnetic
   field vector and acceleration vector should be stored in the buffer by 
   calling #AKFS_Get_MAGNETIC_FIELD and #AKFS_Get_ACCELEROMETER.
  @return The return value is #AKM_SUCCESS when function succeeds. Otherwise
   the return value is #AKM_FAIL.
  @param[out] azimuth Azimuthal angle in degree.
  @param[out] pitch Pitch angle in degree.
  @param[out] roll Roll angle in degree.
  @param[out] accuracy Accuracy of orientation sensor.
 */
int16 AKFS_Get_ORIENTATION(
			AKFLOAT*	azimuth,
			AKFLOAT*	pitch,
			AKFLOAT*	roll,
			int16*		accuracy
)
{
	int16 akret;

	/* Azimuth calculation */
	/* Coordination system follows the Android coordination. */
	akret = AKFS_Direction(
		AKFS_HDATA_SIZE,
		g_prms.mfv_hvbuf,
		CSPEC_HNAVE_D,
		AKFS_ADATA_SIZE,
		g_prms.mfv_avbuf,
		CSPEC_ANAVE_D,
		&g_prms.mf_azimuth,
		&g_prms.mf_pitch,
		&g_prms.mf_roll
	);

	if(akret == AKFS_ERROR) {
		AKMERROR;
		return AKM_FAIL;
	}
	*azimuth  = g_prms.mf_azimuth;
	*pitch    = g_prms.mf_pitch;
	*roll     = g_prms.mf_roll;
	*accuracy = g_prms.mi_hstatus;

	/* Debug output */
	AKMDATA(AKMDATA_ORI, "Ori(%d):%8.2f, %8.2f, %8.2f\n",
			*accuracy, *azimuth, *pitch, *roll);

	return AKM_SUCCESS;
}