/*
 * Copyright (C) 2011 The Android Open Source Project
 *
 * 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.
 */

/* $Id: db_utilities_poly.cpp,v 1.2 2010/09/03 12:00:10 bsouthall Exp $ */

#include "db_utilities_poly.h"
#include "db_utilities.h"


/*****************************************************************
*    Lean and mean begins here                                   *
*****************************************************************/

void db_SolveCubic(double *roots,int *nr_roots,double a,double b,double c,double d)
{
    double bp,bp2,cp,dp,q,r,srq;
    double r2_min_q3,theta,bp_through3,theta_through3;
    double cos_theta_through3,sin_theta_through3,min2_cos_theta_plu,min2_cos_theta_min;
    double si_r_srq,A;

    /*For nondegenerate cubics with three roots
    [24 mult 9 add 2sqrt 1acos 1cos=33flops 4func]
    For nondegenerate cubics with one root
    [16 mult 6 add 1sqrt 1qbrt=24flops 3func]*/

    if(a==0.0) db_SolveQuadratic(roots,nr_roots,b,c,d);
    else
    {
        bp=b/a;
        bp2=bp*bp;
        cp=c/a;
        dp=d/a;

        q=(bp2-3.0*cp)/9.0;
        r=(2.0*bp2*bp-9.0*bp*cp+27.0*dp)/54.0;
        r2_min_q3=r*r-q*q*q;
        if(r2_min_q3<0.0)
        {
            *nr_roots=3;
            /*q has to be > 0*/
            srq=sqrt(q);
            theta=acos(db_maxd(-1.0,db_mind(1.0,r/(q*srq))));
            bp_through3=bp/3.0;
            theta_through3=theta/3.0;
            cos_theta_through3=cos(theta_through3);
            sin_theta_through3=sqrt(db_maxd(0.0,1.0-cos_theta_through3*cos_theta_through3));

            /*cos(theta_through3+2*pi/3)=cos_theta_through3*cos(2*pi/3)-sin_theta_through3*sin(2*pi/3)
            = -0.5*cos_theta_through3-sqrt(3)/2.0*sin_theta_through3
            = -0.5*(cos_theta_through3+sqrt(3)*sin_theta_through3)*/
            min2_cos_theta_plu=cos_theta_through3+DB_SQRT3*sin_theta_through3;
            min2_cos_theta_min=cos_theta_through3-DB_SQRT3*sin_theta_through3;

            roots[0]= -2.0*srq*cos_theta_through3-bp_through3;
            roots[1]=srq*min2_cos_theta_plu-bp_through3;
            roots[2]=srq*min2_cos_theta_min-bp_through3;
        }
        else if(r2_min_q3>0.0)
        {
            *nr_roots=1;
            A= -db_sign(r)*db_CubRoot(db_absd(r)+sqrt(r2_min_q3));
            bp_through3=bp/3.0;
            if(A!=0.0) roots[0]=A+q/A-bp_through3;
            else roots[0]= -bp_through3;
        }
        else
        {
            *nr_roots=2;
            bp_through3=bp/3.0;
            /*q has to be >= 0*/
            si_r_srq=db_sign(r)*sqrt(q);
            /*Single root*/
            roots[0]= -2.0*si_r_srq-bp_through3;
            /*Double root*/
            roots[1]=si_r_srq-bp_through3;
        }
    }
}

void db_SolveQuartic(double *roots,int *nr_roots,double a,double b,double c,double d,double e)
{
    /*Normalized coefficients*/
    double c0,c1,c2,c3;
    /*Temporary coefficients*/
    double c3through2,c3through4,c3c3through4_min_c2,min4_c0;
    double lz,ms,ns,mn,m,n,lz_through2;
    /*Cubic polynomial roots, nr of roots and coefficients*/
    double c_roots[3];
    int nr_c_roots;
    double k0,k1;
    /*nr additional roots from second quadratic*/
    int addroots;

    /*For nondegenerate quartics
    [16mult 11add 2sqrt 1cubic 2quadratic=74flops 8funcs]*/

    if(a==0.0) db_SolveCubic(roots,nr_roots,b,c,d,e);
    else if(e==0.0)
    {
        db_SolveCubic(roots,nr_roots,a,b,c,d);
        roots[*nr_roots]=0.0;
        *nr_roots+=1;
    }
    else
    {
        /*Compute normalized coefficients*/
        c3=b/a;
        c2=c/a;
        c1=d/a;
        c0=e/a;
        /*Compute temporary coefficients*/
        c3through2=c3/2.0;
        c3through4=c3/4.0;
        c3c3through4_min_c2=c3*c3through4-c2;
        min4_c0= -4.0*c0;
        /*Compute coefficients of cubic*/
        k0=min4_c0*c3c3through4_min_c2-c1*c1;
        k1=c1*c3+min4_c0;
        /*k2= -c2*/
        /*k3=1.0*/

        /*Solve it for roots*/
        db_SolveCubic(c_roots,&nr_c_roots,1.0,-c2,k1,k0);

        if(nr_c_roots>0)
        {
            lz=c_roots[0];
            lz_through2=lz/2.0;
            ms=lz+c3c3through4_min_c2;
            ns=lz_through2*lz_through2-c0;
            mn=lz*c3through4-c1/2.0;

            if((ms>=0.0)&&(ns>=0.0))
            {
                m=sqrt(ms);
                n=sqrt(ns)*db_sign(mn);

                db_SolveQuadratic(roots,nr_roots,
                    1.0,c3through2+m,lz_through2+n);

                db_SolveQuadratic(&roots[*nr_roots],&addroots,
                    1.0,c3through2-m,lz_through2-n);

                *nr_roots+=addroots;
            }
            else *nr_roots=0;
        }
        else *nr_roots=0;
    }
}

void db_SolveQuarticForced(double *roots,int *nr_roots,double a,double b,double c,double d,double e)
{
    /*Normalized coefficients*/
    double c0,c1,c2,c3;
    /*Temporary coefficients*/
    double c3through2,c3through4,c3c3through4_min_c2,min4_c0;
    double lz,ms,ns,mn,m,n,lz_through2;
    /*Cubic polynomial roots, nr of roots and coefficients*/
    double c_roots[3];
    int nr_c_roots;
    double k0,k1;
    /*nr additional roots from second quadratic*/
    int addroots;

    /*For nondegenerate quartics
    [16mult 11add 2sqrt 1cubic 2quadratic=74flops 8funcs]*/

    if(a==0.0) db_SolveCubic(roots,nr_roots,b,c,d,e);
    else if(e==0.0)
    {
        db_SolveCubic(roots,nr_roots,a,b,c,d);
        roots[*nr_roots]=0.0;
        *nr_roots+=1;
    }
    else
    {
        /*Compute normalized coefficients*/
        c3=b/a;
        c2=c/a;
        c1=d/a;
        c0=e/a;
        /*Compute temporary coefficients*/
        c3through2=c3/2.0;
        c3through4=c3/4.0;
        c3c3through4_min_c2=c3*c3through4-c2;
        min4_c0= -4.0*c0;
        /*Compute coefficients of cubic*/
        k0=min4_c0*c3c3through4_min_c2-c1*c1;
        k1=c1*c3+min4_c0;
        /*k2= -c2*/
        /*k3=1.0*/

        /*Solve it for roots*/
        db_SolveCubic(c_roots,&nr_c_roots,1.0,-c2,k1,k0);

        if(nr_c_roots>0)
        {
            lz=c_roots[0];
            lz_through2=lz/2.0;
            ms=lz+c3c3through4_min_c2;
            ns=lz_through2*lz_through2-c0;
            mn=lz*c3through4-c1/2.0;

            if(ms<0.0) ms=0.0;
            if(ns<0.0) ns=0.0;

            m=sqrt(ms);
            n=sqrt(ns)*db_sign(mn);

            db_SolveQuadratic(roots,nr_roots,
                1.0,c3through2+m,lz_through2+n);

            db_SolveQuadratic(&roots[*nr_roots],&addroots,
                1.0,c3through2-m,lz_through2-n);

            *nr_roots+=addroots;
        }
        else *nr_roots=0;
    }
}