/*
    PZT_poze.c implements POZE object management, and calculations in the 
    frequency domain. See API headerfile PZT_poze.h.
    PoZeTools v 0.53, december 19, 2006. Copyright (c) 2002-2006 Pieter Suurmond.

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to
    deal in the Software without restriction, including without limitation the
    rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
    sell copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions: 

      The above copyright notice and this permission notice shall be included in
      all copies or substantial portions of the Software. 
      Any person wishing to distribute modifications to the Software is 
      requested to send the modifications to the original developer so that they
      can be incorporated into the canonical version. 

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND ON INFRINGEMENT. 
    IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
    DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 
    OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE 
    USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

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

#include "PZT_complex_math.h"
#include "PZT_complex_text.h"
#include "PZT_poze.h"

/*----------------------------------------------------------------------------------*/
int POZE_read(POZE**      pz,
              short       content, /* 0 if next arg=file. */
              const char* source,  /* filename or content */
              char*       msg)
{
    static const char*  errstr = " ERROR in POZE_read(): ";
    static const int    maxLineLength = 256;   /* Including the \0 character. */
    FILE                *f = (FILE*)NULL;
    char                *lineBuff;
    const char*         mem_src;
    complexCartesian    *p = NULL,      /* Write pointers. */
                        *z = NULL;
    complexCartesian    recC;           /* To receive complex numbers in both */
    complexPolar        recP;           /* cartesian and polar format.        */
    int                 m, len,
                        e = 0,
                        linenum = 0,
                        gCount = 0;
    *pz = NULL;
    if (!msg)
        return 1;
    if (!source)
        { sprintf(msg, "%sthird argument NULL!\n", errstr); return 2; }
    if (!content)
        {
        f = fopen(source, "rt");            /* Argument preferably not NULL. */
        if (!f)
            { sprintf(msg, "%s'%s' not found!\n", errstr, source); return 3; }
        }
    else
        mem_src = source;
    lineBuff = malloc(maxLineLength);
    if (!lineBuff)
        { if (f) fclose(f); sprintf(msg, "%snot enough memory!\n", errstr); return 4; }
    *pz = malloc(sizeof(POZE));
    if (!*pz)
        { free(lineBuff); if (f) fclose(f); sprintf(msg, "%snot enough memory!\n", errstr); return 5; }
    (*pz)->zero.number = 0;                /* First count poles + zeros. */
    (*pz)->pole.number = 0;
    while ((  content  && copy_line(lineBuff, maxLineLength, &mem_src)) ||
           ((!content) && fgets(lineBuff, maxLineLength, f)))
        {
        linenum++;
        len = simplifyString(lineBuff);            
        if (len && (lineBuff[0] != '#'))    /* Ignore empty lines and     */
            {                               /* lines that start with '#'. */
            if ((len < 3) || (lineBuff[1] != '='))
                goto syntaxErrFullClean;    /* numbers set,no free's if 0 */
            if (lineBuff[0] == 'z')
                (*pz)->zero.number++;
            else if (lineBuff[0] == 'p')
                (*pz)->pole.number++;
            else if (lineBuff[0] == 'g')
                gCount++;                   /* Save to call free_ be- */
            else                            /* cause number set to 0. */
                {
syntaxErrFullClean:
                sprintf(msg, "%ssyntax in line %d:\n %s\n", errstr, linenum, lineBuff);
                e = 6; goto done;
                }
            }
        }
    if (gCount > 1)
        { sprintf(msg, "%smultiple gains!\n", errstr); e = 7; goto done; }
    if ((*pz)->zero.number)
        {
        z = (*pz)->zero.cartesian = (complexCartesian*)
            malloc ((*pz)->zero.number * sizeof(complexCartesian));
        if (!z)
            { sprintf(msg, "%snot enough memory for poles!\n", errstr); e = 8; goto done; }
        }
    if ((*pz)->pole.number)
        {
        p = (*pz)->pole.cartesian = (complexCartesian*)
            malloc ((*pz)->pole.number * sizeof(complexCartesian));
        if (!p)
            { sprintf(msg, "%snot enough memory for poles!\n", errstr); e = 9; goto done; }
        }
    if (content)
        mem_src = source;
    else
        rewind(f);                          /* Now really load them.  */
    linenum = 0;
    while ((  content  && copy_line(lineBuff, maxLineLength, &mem_src)) ||
           ((!content) && fgets(lineBuff, maxLineLength, f)))            
        {
        linenum++;
        simplifyString(lineBuff);
        if (lineBuff[0] && (lineBuff[0] != '#'))
            {
            if (readPolarAndCartesian(&lineBuff[2], &recP, &recC))
                {
                sprintf(msg, "%sinvalid polar or cartesian notation in line %d:\n %s\n",
                             errstr, linenum, lineBuff); e = 10; goto done;
                }
            if (lineBuff[0] == 'z')
                memcpy(z++, &recC, sizeof(complexCartesian));
            else if (lineBuff[0] == 'p')
                memcpy(p++, &recC, sizeof(complexCartesian));
            else /* if (lineBuff[0] == 'g')       Sure nothing else !? */
                {
                memcpy(&((*pz)->gain), &recP, sizeof(complexCartesian));
                if ((*pz)->gain.mag == 0.0)
                    {
                    sprintf(msg, "%szero gain not allowed!\n", errstr);
                    e = 11; goto done;     /* Gain 0.0 is NOT accepted! */
                    }
                }
            }
        }                               /* Ok messages: */
    m = sprintf(msg, " Read %d zero", (*pz)->zero.number);
    if ((*pz)->zero.number != 1) m += sprintf(msg + m, "s");
    m += sprintf(msg + m, " and %d pole", (*pz)->pole.number);
    if ((*pz)->pole.number != 1) m += sprintf(msg + m, "s");
    m += sprintf(msg + m, ".\n");
    if (gCount == 0)                    /* Only when no gain is found. */
        {
        sprintf(msg + m, " No gain supplied, automatically normalizing.\n");
        POZE_normalized(*pz, 1);      /* 1 means do it: set new gain. */
        }
done:
    if (e)
        POZE_free(pz);
    free(lineBuff); /* Sure it's there. */
    if (f)
        fclose(f);
    return e;
}

/*-----------------------------------------------------------------------------*/
void POZE_free(POZE** pz)
{
    if (!pz)        /* Pass handle instead of pointer so this */
        return;     /* function can CLEAR the reference also. */
    if (*pz)
        {
        if (((*pz)->zero.number) && ((*pz)->zero.cartesian))
            free((*pz)->zero.cartesian);
        if (((*pz)->pole.number) && ((*pz)->pole.cartesian))
            free((*pz)->pole.cartesian);
        free(*pz);
        }
    *pz = NULL;
}

/*----------------------------------------------------------------------------*/
int POZE_write(const POZE*  pz,             /* Supply P, Z and G.  */
               const char*  stamp,          /* Printed as comment. */
               FILE*        dest)           /* File or 'stdout'.   */
{
    const int       maxLineLength = 128;    /* Hopefully enough. */
    char*           lineBuff;
    complexPolar    cp;
    int             n;
    
    if (!(pz && dest))                      /* Just for safety. */
        return 1;
    lineBuff = malloc(maxLineLength);
    if (!lineBuff)
        return 2;
    if (stamp)
        fprintf(dest, "# %s\n", stamp);
    fprintf(dest, "# %d-zero %d-pole filter:\n\n",
            pz->zero.number, pz->pole.number);
    writePolar(&pz->gain, 10, lineBuff);
    fprintf(dest, "G = %s\n\n", lineBuff);                /* Save the gain. */
    for (n = 0; n < pz->zero.number; n++)           /* Choose 10 decimals.  */
        {                                           /* Choose polar format. */
        cartesianToPolar(&pz->zero.cartesian[n], &cp);
        writePolar(&cp, 10, lineBuff);                      /* Save zeroes. */
        fprintf(dest, "Z = %s\n", lineBuff);
        }
    if (n > 1) fprintf(dest, "\n");
    for (n = 0; n < pz->pole.number; n++)
        {
        cartesianToPolar(&pz->pole.cartesian[n], &cp);
        writePolar(&cp, 10, lineBuff);                      /* Save poles.  */
        fprintf(dest, "P = %s\n", lineBuff);
        }
    fprintf(dest, "\n");
    free(lineBuff); /* Sure it is there. */
    return 0;
}

/*---------------------------------------------------------------------------*/
void POZE_response(const POZE             *pz,                /* Z=, P=, G=. */
                   const complexCartesian *z_c,               /* Frequency.  */
                   complexPolar           *H_p)               /* Response.   */
{
    int              n;
    complexCartesian cc;
    complexPolar     pp;
                                                        /* Gain was stored   */
    memcpy(H_p, &pz->gain, sizeof(complexPolar));       /* in polar form!    */
    for (n=0; n < pz->zero.number; n++)            
        {
        memcpy(&cc, z_c, sizeof(complexCartesian));     /* Take a copy of z. */
        cartesianSub(&pz->zero.cartesian[n], &cc);      /* Subtract Zero n.  */
        cartesianToPolar(&cc, &pp);
        polarMultiply(&pp, H_p);
        }
    for (n=0; n < pz->pole.number; n++)
        {
        memcpy(&cc, z_c, sizeof(complexCartesian));     /* Take a copy of z. */
        cartesianSub(&pz->pole.cartesian[n], &cc);      /* Subtract Pole n.  */
        cartesianToPolar(&cc, &pp);
        polarDivide(&pp, H_p);
        }
#if (0) /* In case you want to assure yourself. */
    if (H_p->mag < 0.0)
        printf("\nERROR: .mag is EXPECTED NEVER to become negative!\n\n");
#endif
}


/*----------------------------------------------------------*/
/*
    Called by POZE_response_range() and by POZE_normalized().
*/
static void cmp_store_magn(const complexPolar *z_polar,
                           const POZE         *pz,
                           response_range     *r_range)
{
    complexPolar        H_p;
    complexCartesian    z_cart;

    polarToCartesian(z_polar, &z_cart);
    POZE_response(pz, &z_cart, &H_p);
    if (H_p.mag > r_range->max_magnitude)
        r_range->max_magnitude = H_p.mag;
    else if (H_p.mag < r_range->min_magnitude)
        r_range->min_magnitude = H_p.mag;
}

/*-----------------------------------------------------------------------------*/
void POZE_response_range(const POZE *pz, response_range *r_range)
{
    int                 n;
    complexPolar        z_p, H_p;               /* Frequency in both forms.   */
    complexCartesian    z_c;
    double              step, angle, t,
                        prev_omega = 9999.99,   /* Init prev_ variables just to */
                        prev_angle = 9999.99;   /* surpress compiler warnings.  */
    short               gain_initialised  = 0,
                        angle_initialised = 0,
                        time_initialised  = 0;
    double              tc, time_bottom_clip, time_top_clip;

    step     = kPZT_onePi / 32768.0;        /* Max 32768 calculations. */
    z_p.mag  = 1.0;
    z_p.ang  = 0.0;                         /* Start search at frequency 0. */
    /* Disregard large (especially very negative) times.
       For a causal system (where pz->zero.number <= pz->pole.number), we expect
       no negative delay-time. So we clip the time-bottom to:
       pz->pole.number - pz->zero.number. Hmmmmmm....?...  */
    tc = (double)(pz->pole.number - pz->zero.number); /* This roughly determines the middle. */
    time_bottom_clip = tc - 24;
    time_top_clip    = tc + 24;

    while (z_p.ang < kPZT_onePi + kPZT_epsilon)
        {
        polarToCartesian(&z_p, &z_c);
        POZE_response(pz, &z_c, &H_p);
        if (gain_initialised)
            {
            if      (H_p.mag > r_range->max_magnitude) r_range->max_magnitude = H_p.mag;
            else if (H_p.mag < r_range->min_magnitude) r_range->min_magnitude = H_p.mag;
            }
        else
            {
            r_range->min_magnitude = r_range->max_magnitude = H_p.mag;
            gain_initialised = 1;
            }
        if (H_p.mag >= kPZT_epsilon) /* Don't take invalid phase-results.  */
            {                        /* And don't disturb phase unwrapper. */
            if (angle_initialised)
                {
                angle = phaseUnwrap(prev_angle, H_p.ang);
                if      (angle > r_range->max_angle) r_range->max_angle = angle;
                else if (angle < r_range->min_angle) r_range->min_angle = angle;
                t = (prev_angle - angle) / 
                    (z_p.ang - prev_omega); /* Equal to step, but it may happen that */
                                            /* we skip certain areas around zeros.   */
                if ((t >= time_bottom_clip) && (t <= time_top_clip))
                    {
                    if (time_initialised)
                        {
                        if      (t > r_range->max_time) r_range->max_time = t;
                        else if (t < r_range->min_time) r_range->min_time = t;
                        }
                    else
                        {
                        r_range->min_time = r_range->max_time = t;
                        time_initialised = 1;
                        }
                    }
                }
            else
                {
                r_range->start_omega = z_p.ang;
                angle                =      /* Our own variables get initialised below. */
                r_range->start_angle =      /* First time, we cannot unwrap phase. */
                r_range->min_angle   = 
                r_range->max_angle   = H_p.ang;
                angle_initialised    = 1;
                }                           /* Update variables for first derivative: */
            prev_omega = z_p.ang;           /* Phase input.  */
            prev_angle = angle;             /* Phase output. */
            }
        z_p.ang += step;
        }
    /* Finally try all pole/zero locations, for magnitude peaks and dips only. */
    for (n=0; n < pz->zero.number; n++)            
        {
        cartesianToPolar(&pz->zero.cartesian[n], &z_p);
        z_p.mag = 1.0;                /* Only take that angle, take length 1.0 */
        cmp_store_magn(&z_p, pz, r_range);
        }
    for (n=0; n < pz->pole.number; n++)            
        {
        cartesianToPolar(&pz->pole.cartesian[n], &z_p);
        z_p.mag = 1.0;
        cmp_store_magn(&z_p, pz, r_range);
        }
}


/*----------------------------------------------------------------------------*/
int POZE_causal(const POZE *pz)
{
    if (pz->zero.number > pz->pole.number)
        return 0;
    return 1;
}

/*----------------------------------------------------------------------------*/
int POZE_fir(const POZE *pz)
{
    int n;                  /* DO NOT COUNT POLES AT THE ORIGIN! */

    for (n = 0; n < pz->pole.number; n++)
        if ((pz->pole.cartesian[n].re > 0.0) ||
            (pz->pole.cartesian[n].im > 0.0))
            return 0;
        
    return 1;                   /* Was wrong in previous version (0.49). */
}

/*----------------------------------------------------------------------------*/
int POZE_stable(const POZE *pz)
{
    int n;

    for (n = 0; n < pz->pole.number; n++)
        {
        if (cartesianMagnitude(&pz->pole.cartesian[n]) >= 1.0)
            return 0;
        }
    return 1; /* All poles are located inside the unit circle. */
}

/*-----------------------------------------------------------------------*/
int POZE_cancelled(POZE*    pz,             /* Pole zero cancellation?   */
                   double   e,              /* Distance below we cancel. */
                   short    doit)           /* Only test or also remove? */
{
    int                 p, z, d;
    double              ee = e * e;         /* Distance squared to    */
    complexCartesian    cc;                 /* avoid repeated sqrt's. */
    int                 cancelled = 0;
    
    p = 0;
    while (p < pz->pole.number)
        {
        z = 0;
        while (z < pz->zero.number)
            {            
            memcpy(&cc, &pz->zero.cartesian[z], sizeof(complexCartesian));
            cartesianSub(&pz->pole.cartesian[p], &cc); /* Result in cc. */
            if ((cc.re * cc.re + cc.im * cc.im) < ee)
                {
                cancelled++;
                if (doit)
                    {   /* Remove that pole and that zero from the p/z-arrays. */
                    pz->pole.number--;
                    for (d = p; d < pz->pole.number; d++)
                        pz->pole.cartesian[d] = pz->pole.cartesian[d+1];
                    pz->zero.number--;
                    for (d = z; d < pz->zero.number; d++)
                        pz->zero.cartesian[d] = pz->zero.cartesian[d+1];
                    }   /* Remember that ->pole.number and ->zero.number do not */
                }       /* tell the exact number of floats allocated in memory  */
             z++;       /* anymore! Assume a normal 'free()' doesn't need num.  */
            }
        p++;
        }
    return cancelled;
}

/*-----------------------------------------------------------------------------*/
int POZE_normalized(POZE *pz, short doit)
{                                               /* Gain must be nonzero    */
    response_range      range;                  /* before calling.         */
    int                 n, ok = 0;
    complexPolar        z_p, H_p;               /* Frequency in both forms.   */
    complexCartesian    z_c;
    double              step;

    if (doit)
        {
        pz->gain.ang = 0.0;
        pz->gain.mag = 1.0;
        }                /* Range from 0.0 to PI to search for global min/max. */

    z_p.mag = 1.0;
    z_p.ang = kPZT_onePi;               /* First test exactly Nyquist. */
    polarToCartesian(&z_p, &z_c);
    POZE_response(pz, &z_c, &H_p);
    range.min_magnitude = range.max_magnitude = H_p.mag; /* Init. */
    step = kPZT_onePi / 16384.0;        /* Maximal 16384 calculations. */
    z_p.ang = 0.0;                      /* Then start search at frequency 0. */
    while (z_p.ang < kPZT_onePi)
        {
        cmp_store_magn(&z_p, pz, &range);
        z_p.ang += step;
        }
    /* Then try all exact pole locations, for magnitude peaks. */
    for (n=0; n < pz->pole.number; n++)            
        {
        cartesianToPolar(&pz->pole.cartesian[n], &z_p);
        z_p.mag = 1.0;
        cmp_store_magn(&z_p, pz, &range);
        }
    if ((range.max_magnitude < 1.0001) &&
        (range.max_magnitude > 0.9999))      /* Some deviation allowed. */
        ok = 1;
    if (doit && !ok)                         /* Only divide if needed.  */
        {
        pz->gain.mag /= range.max_magnitude; /* Angle is not changed.   */
        ok = 1;
        }
    return ok;
}