/*
    PZT_poze_graph.c implements COEFF drawing functions. Using the 
    minimal graphic toolkit 'gt' which can set and get RGB-pixels, draw lines,
    etc. and may give output in compressed JPEG format.
    PoZeTools v 0.45, may 8, 2004. Copyright (c) 2002-2004 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 "gt.h"                 /* Minimal JPEG graphics toolkit. */
#include "PZT_complex_math.h"   /* Complex types and arithmetic. */
#include "PZT_complex_text.h"
#include "PZT_poze.h"           /* The COEFF header needs the POZE header. */
#include "PZT_coeff.h"          /* COEFF object containing time domain data. */
#include "PZT_graph.h"
#include "PZT_coeff_graph.h"

/*-------------------------------------------------------------------------------*/
/*
    Complex arrays x and y must have been allocated (and partially filled) by 
    the caller. Array x will be read, y will be written. Take care that x and y 
    have the same size!
    This routine can calc non-causal systems because it can look into the 
    future: x is already filled completely (with impulse or step signal).
    ... we must use time_shift member of COEFF object then ......
*/
static void COEFF_calc_response(COEFF* ab, cartesian_array* x, cartesian_array* y)
{
    int                 n, i, t, tshift;
    complexCartesian    prod;
                                  /* For non-causal systems only. */
    if (ab->a.coeff[0].delay < 0) tshift = ab->a.coeff[0].delay;
    else                          tshift = 0;
    
    for (n = 0; n < x->number; n++)
        {
        y->cartesian[n].re = 0.0;
        y->cartesian[n].im = 0.0;
        for (i = 0; i < ab->a.number; i++)
            {
            t = n - ab->a.coeff[i].delay + tshift; /* Introduce extra delay (for positive time_shift
                                                      values) or ... go into anticipating mode... non-causal?... */
            if ((t >= 0) && (t < x->number))
                {
                memcpy(&prod, &ab->a.coeff[i].cartesian, sizeof(complexCartesian));
                cartesianMultiply(&x->cartesian[t], &prod); /* Product stored in prod. */
                cartesianAdd(&prod, &y->cartesian[n]);  /* Product stored in last arg. */
                }
            }
        for (i = 1; i < ab->b.number; i++)
            {
            t = n - ab->b.coeff[i].delay;       /* t may be null! */
            if (t >= 0)                         /* t will never reach x->number because */
                {                               /* b.coeff[i].delay will never be < 0.  */
                memcpy(&prod, &ab->b.coeff[i].cartesian, sizeof(complexCartesian));
                cartesianMultiply(&y->cartesian[t], &prod);  /* Product stored in prod. */
                cartesianSub(&prod, &y->cartesian[n]);
                }
            }
        cartesianDivide(&ab->b.coeff[0].cartesian, &y->cartesian[n]);  /* Divide by b0. */
        }
}

/*-------------------------------------------------------------------------------*/
static int COEFF_draw_impulse_response(RGBPICp    pic, /* First 2 args not NULL! */
                                       COEFF*     ab,  /* This one is tested though. */
                                       int        xstart,
                                       int        ystart,
                                       int        width,
                                       int        height)
{
    static const int    xmargin = 48;
    static const char*  reimText[2] = { "Re", "Im" };
    int                 reimYbot[2];
    int                 p, N, n, nn, m, mm, x, y, x2, y2, eff_h, xfact, e = 0;
    short               d, im_there = 0;
    double              yy, cmin, cmax, step, yfact;
    unsigned char       rg;
    char                buffy[64];
    cartesian_array     cx, cy;

    if (!ab)          return 1;
    if (height < 150) return 2;
    if (width  < 200) return 3;

    /* See if we need to display the imaginary part, which will leave us less height be-
       cause we'll then split the image. First inspect forward-coeffs, then feedback-coeffs. */
    for (n=0; n < ab->a.number; n++)
        { if (fabs(ab->a.coeff[n].cartesian.im) >= kPZT_epsilon) { im_there = 1; break; } }
    if (!im_there) /* If necessary to test. */
        {
        for (n=0; n < ab->b.number; n++)
            { if (fabs(ab->b.coeff[n].cartesian.im) >= kPZT_epsilon) { im_there = 1; break; } }
        }
    if (im_there)
        {
        eff_h = ((height + 1) >> 1) - 42;
        reimYbot[0] = 16 + ystart + eff_h;  /* Bottom of real plot. */
        reimYbot[1] = reimYbot[0] + 30 + eff_h;     /* Bottom of imaginary plot. */
        }
    else
        {
        eff_h = height - 54;
        reimYbot[0] = reimYbot[1] = ystart + 16 + eff_h; /* (reimYbot[1] not used) */
        }
    /* Determine how many points in time we need for a 'decent' impulse response. */
    N = ab->a.number + (ab->b.number * 10);       /* But we might need many more! */
    if (N < 10)
        N = 10;
    
    /* Determine horizontal spacing. */
    xfact = (width - xmargin) / N;
    if (xfact > 48)
        xfact = 48;
    else if (xfact < 5)
        {
        xfact = 5;
        N = (width - xmargin) / xfact;   /* Sorry but we have to decrease N a bit. */
        }
    /*---------------------------------------------------------------------------------*/    
    cy.number = cx.number = N;
    cx.cartesian = malloc(cx.number * sizeof(complexCartesian)); /* Allocate for input */
    cy.cartesian = malloc(cy.number * sizeof(complexCartesian)); /* output signals.    */
    for (n = 0; n < N; n++)
        {
        if (n) cx.cartesian[n].re = 0.0;   /* Write impulse (or step?...) signal in array. */
        else   cx.cartesian[n].re = 1.0;
        cx.cartesian[n].im = 0.0;
        }
    if (cx.cartesian && cy.cartesian)
        {
        COEFF_calc_response(ab, &cx, &cy);
        cmin = cmax = 0.0;                  /* Search min/max (always include line at y=0). */
        for (n = 0; n < N; n++)             /* Search min/max. */
            {
            if      (cy.cartesian[n].re < cmin) cmin = cy.cartesian[n].re;
            else if (cy.cartesian[n].re > cmax) cmax = cy.cartesian[n].re;
            if      (cy.cartesian[n].im < cmin) cmin = cy.cartesian[n].im; /* Same scale. */
            else if (cy.cartesian[n].im > cmax) cmax = cy.cartesian[n].im;
            }
        e = findScaleMarks(eff_h, &cmin, &cmax, &step);
        if (e)
           e = 2; 
        else
            {
            yfact = ((double)eff_h) / (cmax - cmin); /* Let's hope we don't divide by zero! */
            
            for (d = 0; d <= im_there; d++) /* 0 = (pure) real; 1 = imaginary. */
                {
                /*----------------------- GRID --------------------------------*/
                if (im_there)
                    {
                    gtDrawString(pic, xstart + 4, reimYbot[d] - eff_h - 18, 0,0,0, 0, reimText[d]); /* 1=over, 0=transparant. */
                    gtDrawString(pic, xstart + 4, reimYbot[d] - eff_h - 17, 0,0,0, 0, "  {y   }");
                    gtDrawString(pic, xstart + 4, reimYbot[d] - eff_h - 16, 0,0,0, 0, "    [n]");
                    }
                else
                    {
                    gtDrawString(pic, xstart + 4, reimYbot[d] - eff_h - 18, 0,0,0, 0, "y");
                    gtDrawString(pic, xstart + 4, reimYbot[d] - eff_h - 16, 0,0,0, 0, " [n]");
                    }
                arrow(pic, xstart + 7, reimYbot[d] - eff_h - 2, 1); /* 1 means up. */
                gtDrawString(pic, xstart + (width>>1), reimYbot[d] - eff_h - 14, 0,0,0, 0, "n"); /* 1=over, 0=transparant. */
                arrow(pic, xstart + (width>>1) - 4, reimYbot[d] - eff_h - 8, 0); /* 0 means to the right. */
                x = xstart + xmargin - 1;
                x2 = xstart + xmargin + ((N-1) * xfact) + 1;
                for (yy = cmin; yy < (cmax + kPZT_epsilon); yy += step) /* Discrete time axis and horizontal lines. */
                    {
                    if (fabs(yy) < kPZT_epsilon) rg = 160; else rg = 196; /* Zero somewhat darker blue. */
                    y = reimYbot[d] - (int)(0.5 + (yfact * (yy - cmin)));
                    gtDrawLine(pic, x, y, x2, y, rg,rg,255);

                    sprintf(buffy, "%.4f", yy);
                    m = cutTrailingZeroesAndDot(buffy);
                    y -= 6;
                    if (fabs(yy - cmax) < kPZT_epsilon) y++;
                    gtDrawString(pic, xstart + xmargin - 3 - (6 * m), y, 96,96,255, 0, buffy); /* 1=over, 0=transparant. */
                    }
                if      (xfact <  8) mm = 10;
                else if (xfact < 16) mm = 5;
                else if (xfact < 40) mm = 2;
                else                 mm = 1;
                
                if (ab->a.coeff[0].delay < 0) p = ab->a.coeff[0].delay;
                else                          p = 0;

                for (n = 0; n < N; n++)
                    {
                    nn = n + p;
                    if (nn)
                        {
                        if (nn % mm)
                            rg = 208;
                        else
                            rg = 196;
                        }
                    else
                        rg = 160;                           /* Zero somewhat darker blue. */
                    x = xstart + xmargin + (n * xfact);
                    gtDrawLine(pic, x, reimYbot[d] + 1, x, reimYbot[d] - eff_h - 1, rg,rg,255);
                    if (!(nn % mm))
                        {
                        gtSetPixel(pic, x, reimYbot[d], 160,160,255);
                        m = sprintf(buffy, "%d", nn);
                        gtDrawString(pic, x + 1 - (3 * m), reimYbot[d] + 2, 96,96,255, 0, buffy);
                        }
                    }
                /*--------------------------------------------------------------------*/
                y2 = reimYbot[d] - (int)(0.5 + (yfact * (0.0 - cmin)));
                for (n = 0; n < N; n++)
                    {
                    /* WAS x = xstart + xmargin + ((n + (ab->a.coeff[0].delay - p)) * xfact); */
                    x = xstart + xmargin + (n * xfact);
                    if (d) yy = cy.cartesian[n].im; else yy = cy.cartesian[n].re;
                    y = reimYbot[d] - (int)(0.5 + (yfact * (yy - cmin)));
                    gtDrawLine(pic, x, y, x, y2, 0,0,0);    /* X */
                    gtSetPixel(pic, x-1, y-1, 128,128,128); gtSetPixel(pic, x, y-1, 64,64,64); gtSetPixel(pic, x+1, y-1, 128,128,128);
                    gtSetPixel(pic, x-1, y,   64, 64, 64);                                     gtSetPixel(pic, x+1, y,   64, 64, 64);
                    gtSetPixel(pic, x-1, y+1, 128,128,128); gtSetPixel(pic, x, y-1, 32,32,32); gtSetPixel(pic, x+1, y+1, 128,128,128);
                    }
                }
            }
        }
    else
        e = 1;
    if (cx.cartesian) free(cx.cartesian);
    if (cy.cartesian) free(cy.cartesian);
    return e;
}


/*------------------------------------------------------------------*/
int COEFF_jpeg_impulse_response(COEFF*      ab,
                                int         width,
                                int         height,
                                const char* title,
                                int         q,
                                FILE*       fp)
{
    RGBPICp       PIC;
    int           n, hd;
    char          buffy[256];
    short         e, ee;

    if (gtCreateMemRGB(&PIC, width, height, 255, 255, 255)) /* White bg. */
        return 1;
    if (title)
        {
        hd = 12;                                          /* Top margin for title. */
        gtDrawString(PIC, 2, 0, 128,128,128, 1, title);   /* 1=over,0=transparant. */
        n = 0;
        if (ab->a.coeff[0].delay < 0)
            n += sprintf(buffy, "non-causal ");
        if (ab->b.number > 1)
            n += sprintf(buffy + n, "in");
        n += sprintf(buffy + n, "finite impulse response");
        buffy[0] -= 32; /* Capitalise first letter. */
        n = (1 + width - (6 * n)) >> 1;
        gtDrawString(PIC, n, 0, 0,0,0, 1, buffy);
        }
    else
        hd = 0;
    ee = COEFF_draw_impulse_response(PIC,       ab,           /* Pass flt.coeffs. */
                                     0,         hd,           /* Startx, starty.  */ 
                                     width,     hd + height); /* Width, height.   */
    if (ee)
        {
        sprintf(buffy, "ERROR COEFF_draw_impulse_response() = %d!", ee);
        gtDrawString(PIC, 16, 16, 255,0,0, 1, buffy);
        }
    /*--------------------- SAVE PICTURE TO DISK: -------------------------*/    
    e = gtOutputMemRGB(PIC, fp, q);
    gtDestroyMemRGB(&PIC);
    return (int)e;
}