/* 
 * PROJECT: NyARToolkitCS
 * --------------------------------------------------------------------------------
 * This work is based on the original ARToolKit developed by
 *   Hirokazu Kato
 *   Mark Billinghurst
 *   HITLab, University of Washington, Seattle
 * http://www.hitl.washington.edu/artoolkit/
 *
 * The NyARToolkitCS is C# edition ARToolKit class library.
 * Copyright (C)2008-2009 Ryo Iizuka
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * 
 * For further information please contact.
 *	http://nyatla.jp/nyatoolkit/
 *	<airmail(at)ebony.plala.or.jp> or <nyatla(at)nyatla.jp>
 * 
 */
using System;
using System.Diagnostics;
using System.Text;

namespace jp.nyatla.nyartoolkit.cs.sandbox.x2
{
    public class NyMath
    {
	public const long FIXEDFLOAT24_1=0x1000000L;
	public const long FIXEDFLOAT24_0_25=FIXEDFLOAT24_1/4;
	public const long FIXEDFLOAT16_1=0x10000L;	
	public const long FIXEDFLOAT16_0_25=FIXEDFLOAT16_1/4;	
	public const long FIXEDFLOAT8_1=0x100L;
	
	private const int FIXEDFLOAT16I_1=(int)FIXEDFLOAT16_1;	
	private const int FIXEDFLOAT16I_0_25=(int)FIXEDFLOAT16_1/4;	
	
	
	

	private const int FF16_PI=(int)(Math.PI*FIXEDFLOAT16_1);
	private const int FF16_2PI=(int)(2 *FF16_PI);
	private const int FF16_05PI=(int)(FF16_PI/2);
	/* sinテーブルは0-2PIを1024分割
	 * acosテーブルは0-1を256分割
	 */
	private static int[] sin_table=new int[339];
	private static int[] acos_table=new int[1537];
    private const int SQRT_LOOP = 10;
	/**
	 * http://www.geocities.co.jp/SiliconValley-PaloAlto/5438/
	 * 参考にしました。
	 * 少数点部が16bitの変数の平方根を求めます。
	 * 戻り値の小数点部分は16bitです。
	 * @param i_v
	 * @return
	 */
	public static long sqrtFixdFloat16(long i_ff16)
	{
		long t=0,s;
		s=i_ff16>0?i_ff16:-i_ff16;
		if(i_ff16==0){
			return 0;
		}
		for(int i=SQRT_LOOP;i>0;i--){
			t = s;
			s = (t+((i_ff16<<16)/t))>>1;
			if(s==t){
				break;
			}
		};
		return t;
	}
	public static long sqrtFixdFloat(long i_ff,int i_bit)
	{
		long t=0,s;
		s=i_ff>0?i_ff:-i_ff;
		if(i_ff==0){
			return 0;
		}
		for(int i=SQRT_LOOP;i>0;i--){
			t = s;
			s = (t+((i_ff<<i_bit)/t))>>1;
			if(s==t){
				break;
			}
		}
		return t;
	}
	public static int acosFixedFloat16(int i_ff24)
	{/*	
		long x=i_ff24>>8;
		long x2=(x*x)>>16;
		long x3=(x2*x)>>16;
		long x4=(x2*x2)>>16;
//		return FF16_05PI-(int)(x+x3/6+(((3*x3*x2/(2*4*5)+(3*5*x4*x3)/(2*4*6*7)))>>16));
*/		
		int result;
		int abs_ff24=i_ff24>0?i_ff24:-i_ff24;
		//(0<=n<=0.25) 0<=0-16384(65536/4)まではy=PI/2-xので近似
		//(0.25<n<=1/2PI) 16385-65536までは(128ステップ単位)のテーブルを使用				

		if(abs_ff24<FIXEDFLOAT24_0_25){
			//0.25までの範囲は、2次の近似式
			result=(i_ff24>>8);
			return FF16_05PI-result+((((result*result)>>16)*result)>>16)/6;
		}else{
			result=acos_table[((abs_ff24>>8)-FIXEDFLOAT16I_0_25)>>5];
			if (i_ff24 < 0) {
				return FF16_PI-result;
			}else{
				return result;
			}
		}
//		return (int)(Math.acos((double)i_ff24/0x1000000)*0x10000);
	}

	public static int sinFixedFloat24(int i_ff16)
	{
		int result;
		//i_ff16を0-2πに制限
		int rad=i_ff16%FF16_2PI;
		if(rad<0){
			rad=rad+FF16_2PI;
		}
		//4ブロックに分割
		int dv=rad/FF16_05PI;
		//radを0-0.5PIに制限
		rad=rad-dv*FF16_05PI;
		//radをdvにより補正
		if(dv==1 || dv==3){
			rad=FF16_05PI-rad;
		}
		//(0<=n<=0.25) 0<=0-16384(65536/4)まではy=xので近似
		//(0.25<n<=1/2PI) 16385-102944までは(256ステップ単位)のテーブルを使用				
		//負にする
		if(rad<FIXEDFLOAT16_0_25){
			result=rad<<8;
		}else{
			result=sin_table[(rad-FIXEDFLOAT16I_0_25)>>8];
		}
		if(dv>=2){
			result=-result;
		}
		return result;
//		return (int)(Math.sin((double)i_ff16/0x10000)*0x1000000);
	}
	public static int cosFixedFloat24(int i_ff16)
	{
		int result;
		//i_ff16を0-2πに制限
		int rad=(i_ff16+FF16_05PI)%FF16_2PI;
		if(rad<0){
			rad=rad+FF16_2PI;
		}
		//4ブロックに分割
		int dv=rad/FF16_05PI;
		//radを0-0.5PIに制限
		rad=rad-dv*FF16_05PI;
		//radをdvにより補正
		if(dv==1 || dv==3){
			rad=FF16_05PI-rad;
		}
		//(0<=n<=0.25) 0<=0-16384(65536/4)まではy=xので近似
		//(0.25<n<=1/2PI) 16385-102944までは(256ステップ単位)のテーブルを使用				
		//負にする
		if(rad<FIXEDFLOAT16_0_25){
			result=rad<<8;
		}else{
			result=sin_table[(rad-FIXEDFLOAT16I_0_25)>>8];
		}
		if(dv>=2){
			result=-result;
		}
		return result;
//		return (int)(Math.cos((double)i_ff16/0x10000)*0x1000000);
	}
	public static void initialize()
	{

		int step;
		step=FIXEDFLOAT16I_0_25+256;
		for(int i=0;i<339;i++){
			sin_table[i]=(int)((Math.Sin((double) step / (double) FIXEDFLOAT16I_1))*FIXEDFLOAT24_1);
			step+=256;
		}
		//acosテーブル初期化
		step=FIXEDFLOAT16I_0_25+32;
		for (int i = 0; i < 1537; i++) {
			acos_table[i] =(int)((Math.Acos((double) step/(double) FIXEDFLOAT16I_1))*FIXEDFLOAT16_1);
			step+=32;
		}
		return;
	}
/*
	public static void printF16(long i_value)
	{
		System.out.println((double)i_value/0x10000);
		return;
	}
	public static void printF24(long i_value)
	{
		System.out.println((double)i_value/0x1000000);
		return;
	}*/
    }
}