/*
 *   Copyright (C) 2002-2006  Felipe Rivera <liebremx at users.sourceforge.net>
 *
 *   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 2 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, write to the Free Software
 *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 *
 *   Calculate coefficients for the IIR filters of the equalizer
 *
 *   $Id: calc_bands.c,v 1.3 2006/01/15 00:17:46 liebremx Exp $
 */

#include <stdio.h>
#include <math.h>
#include <gsl/gsl_math.h>

const char *band_names_11k[] =
{
    "31", "62", "125", "250", "500", 
    "1k", "2k", "3k", "4k", "5.5k" 
};
const double band_f011k[] =
{
    31, 62, 125, 250, 500, 1000, 
    2000, 3000, 4000, 5500 
};
const char *band_names_22k[] =
{
    "31", "62", "125", "250", "500", 
    "1k", "2k", "4k", "8k", "11k" 
};
const double band_f022k[] =
{
    31, 62, 125, 250, 500, 1000, 
    2000, 4000, 8000, 11000 
};
const char *band_names10[] =
{
    "31", "62", "125", "250", "500", 
    "1k", "2k", "4k", "8k", "16k" 
};
const double band_f010[] =
{
    31, 62, 125, 250, 500, 1000, 
    2000, 4000, 8000, 16000 
};

/* Original frequencies in the ten band eq in XMMS */
const char *band_names_original10[] =
{
    "60", "170", "310", "600", "1k", 
    "3k", "6k", "12k", "14k", "16k" 
};
const double band_original_f010[] =
{
    60, 170, 310, 600, 1000, 3000, 
    6000, 12000, 14000, 16000 
};

const char *band_names15[] =
{
    "25", "40", "63", "100", "160", 
    "250", "400", "630", "1k", "1.6k", 
    "2.5k", "4k", "6.3k", "10k", "16k" 
};
const double band_f015[] =
{
    25,40,63,100,160,250,400,630,
    1000,1600,2500,4000,6300,10000,16000
};

const char *band_names25[] =
{
    "20", "31.5", "40" , "50",
    "80", "100", "125", "160", 
    "250", "315", "400", "500",
    "800", "1k", "1.25k", "1.6k", 
    "2.5k", "3.15k", "4k", "5k", 
    "8k", "10k", "12.5k", "16k", "20k" 
};
const double band_f025[] =
{
    20,31.5,40,50,80,100,125,
    160,250,315,400,500,800,
    1000,1250,1600,2500,3150,
    4000,5000,8000,10000,12500,16000,20000
};

const char *band_names31[] =
{
    "20", "25", "31.5", "40" , "50",
    "63", "80", "100", "125", "160", 
    "200", "250", "315", "400", "500",
    "630", "800", "1k", "1.25k", "1.6k", 
    "2k", "2.5k", "3.15k", "4k", "5k", 
    "6.3k", "8k", "10k", "12.5k", "16k", "20k" 
};
const double band_f031[] =
{
    20,25,31.5,40,50,63,80,100,125,
    160,200,250,315,400,500,630,800,
    1000,1250,1600,2000,2500,3150,
    4000,5000,6300,8000,10000,12500,16000,20000
};

#define GAIN_F0 1.0
#define GAIN_F1 GAIN_F0 / M_SQRT2

#define SAMPLING_FREQ 44100.0
#define TETA(f) (2*M_PI*(double)f/sfreq)
#define TWOPOWER(value) (value * value)

#define BETA2(tf0, tf) \
(TWOPOWER(GAIN_F1)*TWOPOWER(cos(tf0)) \
 - 2.0 * TWOPOWER(GAIN_F1) * cos(tf) * cos(tf0) \
 + TWOPOWER(GAIN_F1) \
 - TWOPOWER(GAIN_F0) * TWOPOWER(sin(tf)))
#define BETA1(tf0, tf) \
    (2.0 * TWOPOWER(GAIN_F1) * TWOPOWER(cos(tf)) \
     + TWOPOWER(GAIN_F1) * TWOPOWER(cos(tf0)) \
     - 2.0 * TWOPOWER(GAIN_F1) * cos(tf) * cos(tf0) \
     - TWOPOWER(GAIN_F1) + TWOPOWER(GAIN_F0) * TWOPOWER(sin(tf)))
#define BETA0(tf0, tf) \
    (0.25 * TWOPOWER(GAIN_F1) * TWOPOWER(cos(tf0)) \
     - 0.5 * TWOPOWER(GAIN_F1) * cos(tf) * cos(tf0) \
     + 0.25 * TWOPOWER(GAIN_F1) \
     - 0.25 * TWOPOWER(GAIN_F0) * TWOPOWER(sin(tf)))

#define GAMMA(beta, tf0) ((0.5 + beta) * cos(tf0))
#define ALPHA(beta) ((0.5 - beta)/2.0)

    /* Get the freqs at both sides of F0. These will be cut at -3dB */
void find_f1_and_f2(double f0, double octave_percent, double *f1, double *f2)
{
    double octave_factor = pow(2.0, octave_percent/2.0);
    *f1 = f0/octave_factor;
    *f2 = f0*octave_factor;
}

#define BAND_GROUPS 7
struct _bands {
    const double *cfs;
    double octave;
    const char **band_names;
    int band_count;
    char *band_struct_name;
    int rates;
    double sfreq[5];
};

struct _bands bands[BAND_GROUPS] = {
    {
        band_f011k, 
        1.0, 
        band_names_11k, 
        10,
        "10_11k",
        1,
        { 11025.0 }
    },
    {
        band_f022k, 
        1.0, 
        band_names_22k, 
        10,
        "10_22k",
        1, { 22050.0 }
    },
    {
        band_original_f010, 
        1.0, 
        band_names_original10, 
        10,
        "original10",
        2, { 44100.0, 48000.0 }
    },
    {
        band_f010, 
        1.0, 
        band_names10, 
        10,
        "10",
        2, { 44100.0, 48000.0 }
    },
    {
        band_f015, 
        2.0/3.0, 
        band_names15, 
        15,
        "15",
        2, { 44100.0, 48000.0 }
    },
    {
        band_f025, 
        1.0/3.0,
        band_names25, 
        25,
        "25",
        2, { 44100.0, 48000.0 }
    },
    { 
        band_f031, 
        1.0/3.0,
        band_names31,
        31,
        "31",
        2, { 44100.0, 48000.0 }
    }
};

int find_root(double a, double b, double c, double *x) {
  double k = c-((b*b)/(4*a));
  double h = -(b/(2*a));
  if (-(k/a) < 0.)
    return -1; 
  *x = h - sqrt(-(k/a));
  return 0;
}

int main(int argc, char *argv[])
{
    FILE *file;
    int i, m, n, roots;
    double sfreq;
    double f1, f2;
    double x0, x1;
    char * struct_sig = "static sIIRCoefficients iir_cf%s_%d[] __attribute__((aligned)) = {\n";


    file = fopen("iir_cf.h", "w+");
    fprintf(file, "\n/* BETA, ALPHA, GAMMA */\n");

    for (n=0; n<BAND_GROUPS; n++)
    {
        double *coeff = (double *)bands[n].cfs;
        char **names = (char **)bands[n].band_names;
        for (m=0; m<bands[n].rates; m++)
        {
            sfreq = bands[n].sfreq[m];
            fprintf(file, struct_sig,bands[n].band_struct_name, (int)sfreq);
            for (i=0; i<bands[n].band_count; i++)
            {
                double alpha, beta, gamma;

                /* Find -3dB frequencies for the center freq */
                find_f1_and_f2(coeff[i], bands[n].octave, &f1, &f2);
                printf("Band %s : [%f, %f, %f]\n",
                        names[i], f1, coeff[i], f2);
                /* Find Beta */
                roots = find_root(
                        BETA2(TETA(coeff[i]), TETA(f1)), 
                        BETA1(TETA(coeff[i]), TETA(f1)), 
                        BETA0(TETA(coeff[i]), TETA(f1)), 
                        &x0);
                if (roots == 0)
                {
                    /* Got a solution, now calculate the rest of the factors */
                    /* Take the smallest root always (which is the first returned
                     * by gsl_poly_solve_quadratic
                     *
                     * NOTE: The IIR equation is
                     *	y[n] = 2 * (alpha*(x[n]-x[n-2]) + gamma*y[n-1] - beta*y[n-2])
                     *  Now the 2 factor has been distributed in the coefficients
                     */
                    x0 = x0;
                    beta = 2.0 * x0;
                    alpha = 2.0 * ALPHA(x0);
                    gamma = 2.0 * GAMMA(x0, TETA(coeff[i]));

                    /* Now write the coefficients */
                    fprintf(file, "\t/* %s Hz*/\n", names[i]);
                    fprintf(file, "{ %.30e, %.30e, %.30e },\n",
                            beta, alpha, gamma);
                }
                else
                    printf("  **** Where are the roots?\n");
            }// for i
            fprintf(file, "};\n");
        }//for m
    }//for n
    fclose(file);
}