/******************************************************************************
 *
 * 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_Direction.h"
#include "AKFS_VNorm.h"
#include "AKFS_Math.h"

/*
  Coordinate system is right-handed.
  X-Axis: from left to right.
  Y-Axis: from bottom to top.
  Z-Axis: from reverse to obverse.

  azimuth: Rotaion around Z axis, with positive values 
  	when y-axis moves toward the x-axis.
  pitch: Rotation around X axis, with positive values
  	when z-axis moves toward the y-axis.
  roll: Rotation around Y axis, with positive values
  	when x-axis moves toward the z-axis.
*/

/*
   This function is used internaly, so output is RADIAN!
 */
static void AKFS_Angle(
	const	AKFVEC*		avec,
			AKFLOAT*	pitch,	/* radian */
			AKFLOAT*	roll	/* radian */
)
{
	AKFLOAT	av;	/* Size of vector */

	av = AKFS_SQRT((avec->u.x)*(avec->u.x) + (avec->u.y)*(avec->u.y) + (avec->u.z)*(avec->u.z));

	*pitch = AKFS_ASIN(-(avec->u.y) / av);
	*roll  = AKFS_ASIN((avec->u.x) / av);
}

/*
   This function is used internaly, so output is RADIAN!
 */
static void AKFS_Azimuth(
	const	AKFVEC*		hvec,
	const	AKFLOAT		pitch,	/* radian */
	const	AKFLOAT		roll,	/* radian */
			AKFLOAT*	azimuth	/* radian */
)
{
	AKFLOAT sinP; /* sin value of pitch angle */
	AKFLOAT cosP; /* cos value of pitch angle */
	AKFLOAT sinR; /* sin value of roll angle */
	AKFLOAT cosR; /* cos value of roll angle */
	AKFLOAT Xh;   /* X axis element of vector which is projected to horizontal plane */
	AKFLOAT Yh;   /* Y axis element of vector which is projected to horizontal plane */

	sinP = AKFS_SIN(pitch);
	cosP = AKFS_COS(pitch);
	sinR = AKFS_SIN(roll);
	cosR = AKFS_COS(roll);

	Yh = -(hvec->u.x)*cosR + (hvec->u.z)*sinR;
	Xh = (hvec->u.x)*sinP*sinR + (hvec->u.y)*cosP + (hvec->u.z)*sinP*cosR;

	/* atan2(y, x) -> divisor and dividend is opposite from mathematical equation. */
	*azimuth = AKFS_ATAN2(Yh, Xh);
}

int16 AKFS_Direction(
	const	int16		nhvec,
	const	AKFVEC		hvec[],
	const	int16		hnave,
	const	int16		navec,
	const	AKFVEC		avec[],
	const	int16		anave,
			AKFLOAT*	azimuth,
			AKFLOAT*	pitch,
			AKFLOAT*	roll
)
{
	AKFVEC have, aave;
	AKFLOAT azimuthRad;
	AKFLOAT pitchRad;
	AKFLOAT rollRad;

	/* arguments check */
	if ((nhvec <= 0) || (navec <= 0) || (hnave <= 0) || (anave <= 0)) {
		return AKFS_ERROR;
	}
	if ((nhvec < hnave) || (navec < anave)) {
		return AKFS_ERROR;
	}

	/* average */
	if (AKFS_VbAve(nhvec, hvec, hnave, &have) != AKFS_SUCCESS) {
		return AKFS_ERROR;
	}
	if (AKFS_VbAve(navec, avec, anave, &aave) != AKFS_SUCCESS) {
		return AKFS_ERROR;
	}

	/* calculate pitch and roll */
	AKFS_Angle(&aave, &pitchRad, &rollRad);

	/* calculate azimuth */
	AKFS_Azimuth(&have, pitchRad, rollRad, &azimuthRad);

	*azimuth = RAD2DEG(azimuthRad);
	*pitch = RAD2DEG(pitchRad);
	*roll = RAD2DEG(rollRad);

	/* Adjust range of azimuth */
	if (*azimuth < 0) {
		*azimuth += 360.0f;
	}

	return AKFS_SUCCESS;
}