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

/*
 * CalcR
 */
static AKFLOAT CalcR(
	const	AKFVEC*	x,
	const	AKFVEC*	y
){
	int16	i;
	AKFLOAT	r;

	r = 0.0;
	for(i = 0; i < 3; i++){
		r += (x->v[i]-y->v[i]) * (x->v[i]-y->v[i]);
	}
	r = sqrt(r);

	return r;
}

/*
 * From4Points2Sphere()
 */
static int16 From4Points2Sphere(
	const	AKFVEC		points[],	/*! (i/o)	: input vectors  */
			AKFVEC*		center,		/*! (o)	: center of sphere   */
			AKFLOAT*	r			/*! (i)	: add/subtract value */
){
	AKFLOAT	dif[3][3];
	AKFLOAT	r2[3];

	AKFLOAT	A;
	AKFLOAT	B;
	AKFLOAT	C;
	AKFLOAT	D;
	AKFLOAT	E;
	AKFLOAT	F;
	AKFLOAT	G;

	AKFLOAT	OU;
	AKFLOAT	OD;

	int16	i, j;

	for(i = 0; i < 3; i++){
		r2[i] = 0.0;
		for(j = 0; j < 3; j++){
			dif[i][j] = points[i].v[j] - points[3].v[j];
			r2[i] += (points[i].v[j]*points[i].v[j]
					- points[3].v[j]*points[3].v[j]);
		}
		r2[i] *= 0.5;
	}

	A = dif[0][0]*dif[2][2] - dif[0][2]*dif[2][0];
	B = dif[0][1]*dif[2][0] - dif[0][0]*dif[2][1];
	C = dif[0][0]*dif[2][1] - dif[0][1]*dif[2][0];
	D = dif[0][0]*r2[2]		- dif[2][0]*r2[0];
	E = dif[0][0]*dif[1][1] - dif[0][1]*dif[1][0];
	F = dif[1][0]*dif[0][2] - dif[0][0]*dif[1][2];
	G = dif[0][0]*r2[1]		- dif[1][0]*r2[0];

	OU = D*E + B*G;
	OD = C*F + A*E;

	if(fabs(OD) < AKFS_EPSILON){
		return -1;
	}

	center->v[2] = OU / OD;

	OU = F*center->v[2] + G;
	OD = E;

	if(fabs(OD) < AKFS_EPSILON){
		return -1;
	}

	center->v[1] = OU / OD;

	OU = r2[0] - dif[0][1]*center->v[1] - dif[0][2]*center->v[2];
	OD = dif[0][0];

	if(fabs(OD) < AKFS_EPSILON){
		return -1;
	}

	center->v[0] = OU / OD;

	*r = CalcR(&points[0], center);

	return 0;

}

/*
 * MeanVar
 */
static void MeanVar(
	const	AKFVEC	v[],			/*!< (i)   : input vectors */
	const	int16	n,				/*!< (i)   : number of vectors */
			AKFVEC*	mean,			/*!< (o)   : (max+min)/2 */
			AKFVEC*	var				/*!< (o)   : variation in vectors */
){
	int16	i;
	int16	j;
	AKFVEC	max;
	AKFVEC	min;

	for(j = 0; j < 3; j++){
		min.v[j] = v[0].v[j];
		max.v[j] = v[0].v[j];
		for(i = 1; i < n; i++){
			if(v[i].v[j] < min.v[j]){
				min.v[j] = v[i].v[j];
			}
			if(v[i].v[j] > max.v[j]){
				max.v[j] = v[i].v[j];
			}
		}
		mean->v[j] = (max.v[j] + min.v[j]) / 2.0;	/*mean */
		var->v[j] = max.v[j] - min.v[j];			/*var  */
	}
}

/*
 * Get4points
 */
static void Get4points(
	const	AKFVEC	v[],			/*!< (i)   : input vectors */
	const	int16	n,				/*!< (i)   : number of vectors */
			AKFVEC	out[]			/*!< (o)   : */
){
	int16	i, j;
	AKFLOAT temp;
	AKFLOAT d;

	AKFVEC	dv[AKFS_HBUF_SIZE];
	AKFVEC	cross;
	AKFVEC	tempv;

	/* out 0 */
	out[0] = v[0];

	/* out 1 */
	d = 0.0;
	for(i = 1; i < n; i++){
		temp = CalcR(&v[i], &out[0]);
		if(d < temp){
			d = temp;
			out[1] = v[i];
		}
	}

	/* out 2 */
	d = 0.0;
	for(j = 0; j < 3; j++){
		dv[0].v[j] = out[1].v[j] - out[0].v[j];
	}
	for(i = 1; i < n; i++){
		for(j = 0; j < 3; j++){
			dv[i].v[j] = v[i].v[j] - out[0].v[j];
		}
		tempv.v[0] = dv[0].v[1]*dv[i].v[2] - dv[0].v[2]*dv[i].v[1];
		tempv.v[1] = dv[0].v[2]*dv[i].v[0] - dv[0].v[0]*dv[i].v[2];
		tempv.v[2] = dv[0].v[0]*dv[i].v[1] - dv[0].v[1]*dv[i].v[0];
		temp =	tempv.u.x * tempv.u.x
			  +	tempv.u.y * tempv.u.y
			  +	tempv.u.z * tempv.u.z;
		if(d < temp){
			d = temp;
			out[2] = v[i];
			cross = tempv;
		}
	}

	/* out 3 */
	d = 0.0;
	for(i = 1; i < n; i++){
		temp =	  dv[i].u.x * cross.u.x
				+ dv[i].u.y * cross.u.y
				+ dv[i].u.z * cross.u.z;
		temp = fabs(temp);
		if(d < temp){
			d = temp;
			out[3] = v[i];
		}
	}
}

/*
 * CheckInitFvec
 */
static int16 CheckInitFvec(
	const	AKFVEC	*v		/*!< [in]	vector */
){
	int16 i;

	for(i = 0; i < 3; i++){
		if(AKFS_FMAX <= v->v[i]){
			return 1;       /* initvalue */
		}
	}

	return 0;       /* not initvalue */
}

/*
 * AKFS_AOC
 */
int16 AKFS_AOC(				/*!< (o)   : calibration success(1), failure(0) */
			AKFS_AOC_VAR*	haocv,	/*!< (i/o)	: a set of variables */
	const	AKFVEC*			hdata,	/*!< (i)	: vectors of data    */
			AKFVEC*			ho		/*!< (i/o)	: offset             */
){
	int16	i, j;
	int16	num;
	AKFLOAT	tempf;
	AKFVEC	tempho;

	AKFVEC	fourpoints[4];

	AKFVEC	var;
	AKFVEC	mean;

	/* buffer new data */
	for(i = 1; i < AKFS_HBUF_SIZE; i++){
		haocv->hbuf[AKFS_HBUF_SIZE-i] = haocv->hbuf[AKFS_HBUF_SIZE-i-1];
	}
	haocv->hbuf[0] = *hdata;

	/* Check Init */
	num = 0;
	for(i = AKFS_HBUF_SIZE; 3 < i; i--){
		if(CheckInitFvec(&haocv->hbuf[i-1]) == 0){
			num = i;
			break;
		}
	}
	if(num < 4){
		return AKFS_ERROR;
	}

	/* get 4 points */
	Get4points(haocv->hbuf, num, fourpoints);

	/* estimate offset */
	if(0 != From4Points2Sphere(fourpoints, &tempho, &haocv->hraoc)){
		return AKFS_ERROR;
	}

	/* check distance */
	for(i = 0; i < 4; i++){
		for(j = (i+1); j < 4; j++){
			tempf = CalcR(&fourpoints[i], &fourpoints[j]);
			if((tempf < haocv->hraoc)||(tempf < AKFS_HR_TH)){
				return AKFS_ERROR;
			}
		}
	}

	/* update offset buffer */
	for(i = 1; i < AKFS_HOBUF_SIZE; i++){
		haocv->hobuf[AKFS_HOBUF_SIZE-i] = haocv->hobuf[AKFS_HOBUF_SIZE-i-1];
	}
	haocv->hobuf[0] = tempho;

	/* clear hbuf */
	for(i = (AKFS_HBUF_SIZE>>1); i < AKFS_HBUF_SIZE; i++) {
		for(j = 0; j < 3; j++) {
			haocv->hbuf[i].v[j]= AKFS_FMAX;
		}
	}

	/* Check Init */
	if(CheckInitFvec(&haocv->hobuf[AKFS_HOBUF_SIZE-1]) == 1){
		return AKFS_ERROR;
	}

	/* Check ovar */
	tempf = haocv->hraoc * AKFS_HO_TH;
	MeanVar(haocv->hobuf, AKFS_HOBUF_SIZE, &mean, &var);
	if ((var.u.x >= tempf) || (var.u.y >= tempf) || (var.u.z >= tempf)){
		return AKFS_ERROR;
	}

	*ho = mean;

	return AKFS_SUCCESS;
}

/*
 * AKFS_InitAOC
 */
void AKFS_InitAOC(
			AKFS_AOC_VAR*	haocv
){
	int16 i, j;

	/* Initialize buffer */
	for(i = 0; i < AKFS_HBUF_SIZE; i++) {
		for(j = 0; j < 3; j++) {
			haocv->hbuf[i].v[j]= AKFS_FMAX;
		}
	}
	for(i = 0; i < AKFS_HOBUF_SIZE; i++) {
		for(j = 0; j < 3; j++) {
			haocv->hobuf[i].v[j]= AKFS_FMAX;
		}
	}

	haocv->hraoc = 0.0;
}