/* accepts two bitreversed numbers, returns the poles of the filter. */
#define BITREV

#include <stdio.h>
#include <math.h>

int iq_rom[128] =
{
    0,      9,      12,     25,     33,     41,     49,     57,
    65,     73,     81,     89,     97,     105,    113,    121,
    129,    137,    145,    153,    161,    169,    177,    185,    
    193,    201,    209,    217,    225,    233,    241,    249,
    257,    265,    273,    281,    289,    297,    301,    305,
    309,    313,    317,    321,    325,    329,    333,    337,
    341,    345,    349,    353,    357,    361,    365,    369,    
    373,    377,    381,    385,    389,    393,    397,    401,
    405,    409,    413,    417,    421,    425,    427,    429,
    431,    433,    435,    437,    439,    441,    443,    445,    
    447,    449,    451,    453,    455,    457,    459,    461,    
    463,    465,    467,    469,    471,    473,    475,    477,
    479,    481,    482,    483,    484,    485,    486,    487,
    488,    489,    490,    491,    492,    493,    494,    495,
    496,    497,    498,    499,    500,    501,    502,    503,
    504,    505,    506,    507,    508,    509,    510,    511
};

typedef struct cplx 
{ 
    double r, i;
    double a, m;
} cplx;

int calc_poles(double Ft, double Bt, cplx *pole, double Fs, 
               double *Fk, double *BW)
{
    double discrim;
    double root;
    int    sign = 0;

    discrim = Ft * Ft + Bt;
    if (discrim < 0) { sign = 1; discrim = -discrim; }
    root    = sqrt(discrim);

    if (sign)
    {
        pole[0].r =  (pole[1].r = Ft);
        pole[0].i = -(pole[1].i = root);

        pole[0].m = pole[1].m = sqrt(Ft*Ft + discrim);
        pole[0].a = atan2(pole[0].i, pole[0].r);
        pole[1].a = atan2(pole[1].i, pole[1].r);

        Fk[0] =  Fs * pole[1].a / (2.0*M_PI);
        BW[0] = -Fs * log(Bt * Bt) / M_PI;
        BW[1] = 0;
    } else
    {
        int i;

        pole[0].i = pole[1].i = 0;
        pole[0].r = Ft + root;
        pole[1].r = Ft - root;

        pole[0].m = fabs(Ft + root);
        pole[1].m = fabs(Ft - root);
        pole[0].a = pole[0].r > 0 ? 0 : M_PI;
        pole[1].a = pole[1].r > 0 ? 0 : M_PI;

        for (i = 0; i < 2; i++)
        {
            if (pole[i].r < 0)
            {
                Fk[i] = Fs / 2.0;
                BW[i] = -2 * Fs * log(-pole[i].r);
            } else
            {
                Fk[i] = 0;
                BW[i] = -2 * Fs * log(pole[i].r);
            }
        }
    }

    return sign;
}

void show_poles(cplx *pole, int num_poles)
{
    int x, y, c, p;
    double bb_x0, bb_x1, bb_y0, bb_y1;
    double sq_x0, sq_x1, sq_y0, sq_y1;
    int num_inside, ws;

    for (p = 0; p < num_poles; p++)
        printf("%2d: [%10f,%10f]   %s\n", 
                p + 1, pole[p].r, pole[p].i,
                pole[p].m >= 1.0 ? "Unstable" : "Stable");

    for (y = -12; y <= +12; y++)
    {
        bb_y0 = y / 10.0 - 0.05;
        bb_y1 = y / 10.0 + 0.05;

        sq_y0 = bb_y0 * bb_y0;
        sq_y1 = bb_y1 * bb_y1;

        ws = 0;

        for (x = -32; x <= +32; x++)
        {
            bb_x0 = x / 20.0 - 0.025;
            bb_x1 = x / 20.0 + 0.025;

            sq_x0 = bb_x0 * bb_x0;
            sq_x1 = bb_x1 * bb_x1;

            c = ' ';

            if (y == 0) c = '-';
            if (x == 0) c = y == 0 ? '+' : '|';

            num_inside  = (sq_x0 + sq_y0) < 1.0;
            num_inside += (sq_x0 + sq_y1) < 1.0;
            num_inside += (sq_x1 + sq_y0) < 1.0;
            num_inside += (sq_x1 + sq_y1) < 1.0;

            if (num_inside == 2 || num_inside == 3) c = '*';

            for (p = 0; p < num_poles; p++)
            {
                if (pole[p].r >= bb_x0 && pole[p].r <= bb_x1 &&
                    pole[p].i >= bb_y0 && pole[p].i <= bb_y1)
                {
                    c = '1' + p;
                }
            }

            if (c == ' ') ws++;
            else
            {
                printf("%*s%c", ws, "", c);
                ws = 0;
            }
        }
        putchar('\n');
    }

}

double bitdecode(char *str)
{
    int val = 0, iq_val;
    int len = 0;
    char *s;
    double final;

#ifdef BITREV
    for (s = str; *s; s++, len++)
    {
        if (*s == '1') val |= 1 << len;
        else if (*s != '0')
        {
            fprintf(stderr, "Format error in '%s'\n", str);
            exit(1);
        }
    }

    if (len > 8)
    {
        fprintf(stderr, "Field too long (max 8): '%s'\n", str);
        exit(1);
    }

    /* Left justify */
    if (len < 8) val <<= 8 - len;
#else
    for (s = str; *s; s++, len++)
    {
        if (*s == '1') val |= 0x80 >> len;
        else if (*s != '0')
        {
            fprintf(stderr, "Format error in '%s'\n", str);
            exit(1);
        }
    }

    if (len > 8)
    {
        fprintf(stderr, "Field too long (max 8): '%s'\n", str);
        exit(1);
    }
#endif


#if 0
    iq_val = iq_rom[val & 0x7F];

    if (val & 0x80) final =  iq_val / 512.0;
    else            final = -iq_val / 512.0;
#else
    if (val & 0x80) final =  iq_rom[(-val) & 0x7F] / 512.0;
    else            final = -iq_rom[  val        ] / 512.0;
#endif

    return final;
}

void calc_impulse_resp(double Ft, double Bt, double *s, int cnt)
{
    double z0, z1, samp;
    int i;
    
    s[0] = samp = 1.0;
    z0 = z1 = 0.0;
    for (i = 1; i < cnt; i++)
    {
        samp = samp + 2.0 * Ft * z0 + Bt * z1;

        z1   = z0;
        z0   = samp;
        s[i] = samp > 1.0 ? 1.0 : samp < -1.0 ? -1.0 : samp;
        samp = 0.0;
    }
}

void show_impulse_resp(double *s, int cnt)
{
    int y, x, c0, c1, ws, uy, ly, my;

    for (y = 12; y >= -12; y--)
    {
        ws = 0;
        for (x = 0; x < cnt; x++)
        {
            c0 = y == 0 ? '-' : ' ';
            c1 = x % 10 == 0 ? '|' : y == 0 ? x % 2 == 0 ? '|' : '-' : ' ';

            if (x == 0)
            {
                uy = ly = my = floor(s[x]   * 10.0 + 0.5);
            } else  if (s[x] > s[x-1]) 
            {
                ly = floor(s[x-1] * 10.0 + 0.5);
                uy = floor(s[x]   * 10.0 + 0.5);
                my = uy;
            } else
            {
                uy = floor(s[x-1] * 10.0 + 0.5);
                ly = floor(s[x]   * 10.0 + 0.5);
                my = ly;
            }

            if (y > ly && y < uy)   c0 = '*';
            if (y == ly || y == uy) c0 = '*';

            if (y == my) c1 = '*';


            if (c0== ' ') ws++;
            else
            {
                printf("%*c", ws+1, c0);
                ws = 0;
            }

            if (c1== ' ') ws++;
            else
            {
                printf("%*c", ws+1, c1);
                ws = 0;
            }
        }
        putchar('\n');
    }
}

main(int argc, char *argv[])
{
    double Fs = 10000;
    double Ft, Bt, Fk[2], BW[2];
    cplx   pole[2];
    int    is_cplx;
    double impulse[80];

    if (argc < 3)
    {
        fprintf(stderr, "usage: %s bitfield bitfield\n", argv[0]);
        exit(1);
    }

    Bt = bitdecode(argv[1]);
    Ft = bitdecode(argv[2]);

    printf("Ft: %10f    Bt: %10f\n", Ft, Bt);

    is_cplx = calc_poles(Ft, Bt, pole, Fs, Fk, BW);
    if (is_cplx) printf("Fk: %10.3fHz  BW: %10.3fHz\n", Fk[0], BW[0]);
    else         
    {
        printf("Fk: %10.3fHz  BW: %10.3fHz\n",Fk[0],BW[0]);
        printf("Fk: %10.3fHz  BW: %10.3fHz\n",Fk[1],BW[1]);
    }
    show_poles(pole, 2);

    calc_impulse_resp(Ft, Bt, impulse, 39);
    show_impulse_resp(impulse, 39);

    return 0;
}

/* ======================================================================== */
/*  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.               */
/* ======================================================================== */
/*                 Copyright (c) 1998-2001, Joseph Zbiciak                  */
/* ======================================================================== */
