
/* Square root, sine, cosine, and arctangent of polynomial.
 * See polyn.c for data structures and discussion.
 */

#include <stdio.h>
#include <stdlib.h>
#include "mconf.h"

/* Highest degree of polynomial to be handled
 * by the polyn.c subroutine package.  */
#define N 16
/* Highest degree actually initialized at runtime.  */
extern int MAXPOL;

/* Taylor series coefficients for various functions
 */
double patan[N + 1] = {
    0.0, 1.0, 0.0, -1.0 / 3.0, 0.0,
    1.0 / 5.0, 0.0, -1.0 / 7.0, 0.0, 1.0 / 9.0, 0.0, -1.0 / 11.0,
    0.0, 1.0 / 13.0, 0.0, -1.0 / 15.0, 0.0
};

double psin[N + 1] = {
    0.0, 1.0, 0.0, -1.0 / 6.0, 0.0, 1.0 / 120.0, 0.0,
    -1.0 / 5040.0, 0.0, 1.0 / 362880.0, 0.0, -1.0 / 39916800.0,
    0.0, 1.0 / 6227020800.0, 0.0, -1.0 / 1.307674368e12, 0.0
};

double pcos[N + 1] = {
    1.0, 0.0, -1.0 / 2.0, 0.0, 1.0 / 24.0, 0.0,
    -1.0 / 720.0, 0.0, 1.0 / 40320.0, 0.0, -1.0 / 3628800.0, 0.0,
    1.0 / 479001600.0, 0.0, -1.0 / 8.7179291e10, 0.0, 1.0 / 2.0922789888e13
};

double pasin[N + 1] = {
    0.0, 1.0, 0.0, 1.0 / 6.0, 0.0,
    3.0 / 40.0, 0.0, 15.0 / 336.0, 0.0, 105.0 / 3456.0, 0.0,
    945.0 / 42240.0,
    0.0, 10395.0 / 599040.0, 0.0, 135135.0 / 9676800.0, 0.0
};

/* Square root of 1 + x.  */
double psqrt[N + 1] = {
    1.0, 1. / 2., -1. / 8., 1. / 16., -5. / 128., 7. / 256., -21. / 1024.,
    33. / 2048.,
    -429. / 32768., 715. / 65536., -2431. / 262144., 4199. / 524288.,
    -29393. / 4194304.,
    52003. / 8388608., -185725. / 33554432., 334305. / 67108864.,
    -9694845. / 2147483648.
};

/* Arctangent of the ratio num/den of two polynomials.
 */
void polatn(num, den, ans, nn)
double num[], den[], ans[];
int nn;
{
    double a, t;
    double *polq, *polu, *polt;
    int i;

    if (nn > N) {
	mtherr("polatn", OVERFLOW);
	return;
    }
    /* arctan( a + b ) = arctan(a) + arctan( b/(1 + ab + a**2) ) */
    t = num[0];
    a = den[0];
    if ((t == 0.0) && (a == 0.0)) {
	t = num[1];
	a = den[1];
    }
    t = atan2(t, a);		/* arctan(num/den), the ANSI argument order */
    polq = (double *) malloc((MAXPOL + 1) * sizeof(double));
    polu = (double *) malloc((MAXPOL + 1) * sizeof(double));
    polt = (double *) malloc((MAXPOL + 1) * sizeof(double));
    polclr(polq, MAXPOL);
    i = poldiv(den, nn, num, nn, polq);
    a = polq[0];		/* a */
    polq[0] = 0.0;		/* b */
    polmov(polq, nn, polu);	/* b */
    /* Form the polynomial
     * 1 + ab + a**2
     * where a is a scalar.  */
    for (i = 0; i <= nn; i++)
	polu[i] *= a;
    polu[0] += 1.0 + a * a;
    poldiv(polu, nn, polq, nn, polt);	/* divide into b */
    polsbt(polt, nn, patan, nn, polu);	/* arctan(b)  */
    polu[0] += t;		/* plus arctan(a) */
    polmov(polu, nn, ans);
    free(polt);
    free(polu);
    free(polq);
}



/* Square root of a polynomial.
 * Assumes the lowest degree nonzero term is dominant
 * and of even degree.  An error message is given
 * if the Newton iteration does not converge.
 */
void polsqt(pol, ans, nn)
double pol[], ans[];
int nn;
{
    double t;
    double *x, *y;
    int i, n;

#if 0
    double z[N + 1];
    double u;
#endif

    if (nn > N) {
	mtherr("polatn", OVERFLOW);
	return;
    }
    x = (double *) malloc((MAXPOL + 1) * sizeof(double));
    y = (double *) malloc((MAXPOL + 1) * sizeof(double));
    polmov(pol, nn, x);
    polclr(y, MAXPOL);

    /* Find lowest degree nonzero term.  */
    t = 0.0;
    for (n = 0; n < nn; n++) {
	if (x[n] != 0.0)
	    goto nzero;
    }
    polmov(y, nn, ans);
    return;

  nzero:

    if (n > 0) {
	if (n & 1) {
	    printf("error, sqrt of odd polynomial\n");
	    return;
	}
	/* Divide by x^n.  */
	y[n] = x[n];
	poldiv(y, nn, pol, N, x);
    }

    t = x[0];
    for (i = 1; i <= nn; i++)
	x[i] /= t;
    x[0] = 0.0;
    /* series development sqrt(1+x) = 1  +  x / 2  -  x**2 / 8  +  x**3 / 16
     * hopes that first (constant) term is greater than what follows   */
    polsbt(x, nn, psqrt, nn, y);
    t = sqrt(t);
    for (i = 0; i <= nn; i++)
	y[i] *= t;

    /* If first nonzero coefficient was at degree n > 0, multiply by
     * x^(n/2).  */
    if (n > 0) {
	polclr(x, MAXPOL);
	x[n / 2] = 1.0;
	polmul(x, nn, y, nn, y);
    }
#if 0
    /* Newton iterations */
    for (n = 0; n < 10; n++) {
	poldiv(y, nn, pol, nn, z);
	poladd(y, nn, z, nn, y);
	for (i = 0; i <= nn; i++)
	    y[i] *= 0.5;
	for (i = 0; i <= nn; i++) {
	    u = fabs(y[i] - z[i]);
	    if (u > 1.0e-15)
		goto more;
	}
	goto done;
      more:;
    }
    printf("square root did not converge\n");
  done:
#endif				/* 0 */

    polmov(y, nn, ans);
    free(y);
    free(x);
}



/* Sine of a polynomial.
 * The computation uses
 *     sin(a+b) = sin(a) cos(b) + cos(a) sin(b)
 * where a is the constant term of the polynomial and
 * b is the sum of the rest of the terms.
 * Since sin(b) and cos(b) are computed by series expansions,
 * the value of b should be small.
 */
void polsin(x, y, nn)
double x[], y[];
int nn;
{
    double a, sc;
    double *w, *c;
    int i;

    if (nn > N) {
	mtherr("polatn", OVERFLOW);
	return;
    }
    w = (double *) malloc((MAXPOL + 1) * sizeof(double));
    c = (double *) malloc((MAXPOL + 1) * sizeof(double));
    polmov(x, nn, w);
    polclr(c, MAXPOL);
    polclr(y, nn);
    /* a, in the description, is x[0].  b is the polynomial x - x[0].  */
    a = w[0];
    /* c = cos (b) */
    w[0] = 0.0;
    polsbt(w, nn, pcos, nn, c);
    sc = sin(a);
    /* sin(a) cos (b) */
    for (i = 0; i <= nn; i++)
	c[i] *= sc;
    /* y = sin (b)  */
    polsbt(w, nn, psin, nn, y);
    sc = cos(a);
    /* cos(a) sin(b) */
    for (i = 0; i <= nn; i++)
	y[i] *= sc;
    poladd(c, nn, y, nn, y);
    free(c);
    free(w);
}


/* Cosine of a polynomial.
 * The computation uses
 *     cos(a+b) = cos(a) cos(b) - sin(a) sin(b)
 * where a is the constant term of the polynomial and
 * b is the sum of the rest of the terms.
 * Since sin(b) and cos(b) are computed by series expansions,
 * the value of b should be small.
 */
void polcos(x, y, nn)
double x[], y[];
int nn;
{
    double a, sc;
    double *w, *c;
    int i;

    if (nn > N) {
	mtherr("polatn", OVERFLOW);
	return;
    }
    w = (double *) malloc((MAXPOL + 1) * sizeof(double));
    c = (double *) malloc((MAXPOL + 1) * sizeof(double));
    polmov(x, nn, w);
    polclr(c, MAXPOL);
    polclr(y, nn);
    a = w[0];
    w[0] = 0.0;
    /* c = cos(b)  */
    polsbt(w, nn, pcos, nn, c);
    sc = cos(a);
    /* cos(a) cos(b)  */
    for (i = 0; i <= nn; i++)
	c[i] *= sc;
    /* y = sin(b) */
    polsbt(w, nn, psin, nn, y);
    sc = sin(a);
    /* sin(a) sin(b) */
    for (i = 0; i <= nn; i++)
	y[i] *= sc;
    polsub(y, nn, c, nn, y);
    free(c);
    free(w);
}
