/*-
 * Copyright (c) 2015 Dag-Erling Smørgrav
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 *
 * $FreeBSD$
 */

#ifdef _KERNEL
#include <sys/libkern.h>
#else
#include <stdio.h>
#include <strings.h>
#endif

#include "fp16.h"

/*
 * Compute the quare root of x, using Newton's method with 2^(log2(x)/2)
 * as the initial estimate.
 */
fp16_t
fp16_sqrt(fp16_t x)
{
	fp16_t y, delta;
	signed int log2x;

	/* special case */
	if (x == 0)
		return (0);

	/* shift toward 0 by half the logarithm */
	log2x = flsl(x) - 1;
	if (log2x >= 16) {
		y = x >> (log2x - 16) / 2;
	} else {
#if 0
		y = x << (16 - log2x) / 2;
#else
		/* XXX for now, return 0 for anything < 1 */
		return (0);
#endif
	}
	while (y > 0) {
		/* delta = y^2 / 2y */
		delta = fp16_div(fp16_sub(fp16_mul(y, y), x), y * 2);
		if (delta == 0)
			break;
		y = fp16_sub(y, delta);
	}
	return (y);
}

static fp16_t fp16_sin_table[256] = {
	    0,	  402,	  804,	 1206,	 1608,	 2010,	 2412,	 2814,
	 3215,	 3617,	 4018,	 4420,	 4821,	 5222,	 5622,	 6023,
	 6423,	 6823,	 7223,	 7623,	 8022,	 8421,	 8819,	 9218,
	 9616,	10013,	10410,	10807,	11204,	11600,	11995,	12390,
	12785,	13179,	13573,	13966,	14359,	14751,	15142,	15533,
	15923,	16313,	16702,	17091,	17479,	17866,	18253,	18638,
	19024,	19408,	19792,	20175,	20557,	20938,	21319,	21699,
	22078,	22456,	22833,	23210,	23586,	23960,	24334,	24707,
	25079,	25450,	25820,	26189,	26557,	26925,	27291,	27656,
	28020,	28383,	28745,	29105,	29465,	29824,	30181,	30538,
	30893,	31247,	31600,	31952,	32302,	32651,	32999,	33346,
	33692,	34036,	34379,	34721,	35061,	35400,	35738,	36074,
	36409,	36743,	37075,	37406,	37736,	38064,	38390,	38716,
	39039,	39362,	39682,	40002,	40319,	40636,	40950,	41263,
	41575,	41885,	42194,	42501,	42806,	43110,	43412,	43712,
	44011,	44308,	44603,	44897,	45189,	45480,	45768,	46055,
	46340,	46624,	46906,	47186,	47464,	47740,	48015,	48288,
	48558,	48828,	49095,	49360,	49624,	49886,	50146,	50403,
	50660,	50914,	51166,	51416,	51665,	51911,	52155,	52398,
	52639,	52877,	53114,	53348,	53581,	53811,	54040,	54266,
	54491,	54713,	54933,	55152,	55368,	55582,	55794,	56004,
	56212,	56417,	56621,	56822,	57022,	57219,	57414,	57606,
	57797,	57986,	58172,	58356,	58538,	58718,	58895,	59070,
	59243,	59414,	59583,	59749,	59913,	60075,	60235,	60392,
	60547,	60700,	60850,	60998,	61144,	61288,	61429,	61568,
	61705,	61839,	61971,	62100,	62228,	62353,	62475,	62596,
	62714,	62829,	62942,	63053,	63162,	63268,	63371,	63473,
	63571,	63668,	63762,	63854,	63943,	64030,	64115,	64197,
	64276,	64353,	64428,	64501,	64571,	64638,	64703,	64766,
	64826,	64884,	64939,	64992,	65043,	65091,	65136,	65179,
	65220,	65258,	65294,	65327,	65358,	65386,	65412,	65436,
	65457,	65475,	65491,	65505,	65516,	65524,	65531,	65534,
};

/*
 * Compute the sine of theta.
 */
fp16_t
fp16_sin(fp16_t theta)
{
	unsigned int i;

	i = 1024 * (theta % FP16_2PI) / FP16_2PI;
	switch (i / 256) {
	case 0:
		return (fp16_sin_table[i % 256]);
	case 1:
		return (fp16_sin_table[255 - i % 256]);
	case 2:
		return (-fp16_sin_table[i % 256]);
	case 3:
		return (-fp16_sin_table[255 - i % 256]);
	default:
		/* inconceivable! */
		return (0);
	}
}

/*
 * Compute the cosine of theta.
 */
fp16_t
fp16_cos(fp16_t theta)
{
	unsigned int i;

	i = 1024 * (theta % FP16_2PI) / FP16_2PI;
	switch (i / 256) {
	case 0:
		return (fp16_sin_table[255 - i % 256]);
	case 1:
		return (-fp16_sin_table[i % 256]);
	case 2:
		return (-fp16_sin_table[255 - i % 256]);
	case 3:
		return (fp16_sin_table[i % 256]);
	default:
		/* inconceivable! */
		return (0);
	}
}