[PD-cvs] externals/pdp/modules/matrix_basic Makefile, 1.2, 1.3 README, 1.2, 1.3 clusterstuff.c, 1.2, 1.3 pdp_mat_lu.c, 1.2, 1.3 pdp_mat_mul.c, 1.2, 1.3 pdp_mat_vec.c, 1.2, 1.3

[Hans-Christoph Steiner]

Update of /cvsroot/pure-data/externals/pdp/modules/matrix_basic
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv6756/modules/matrix_basic

Added Files:
	Makefile README clusterstuff.c pdp_mat_lu.c pdp_mat_mul.c 
	pdp_mat_vec.c 
Log Message:
checking in pdp 0.12.4 from http://zwizwa.fartit.com/pd/pdp/pdp-0.12.4.tar.gz

--- NEW FILE: pdp_mat_mul.c ---
/*
 *   Pure Data Packet module. Matrix multiplication module
 *   Copyright (c) by Tom Schouten <pdp at zzz.kotnet.org>
 *
 *   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.
 *
 */

//#include <gsl/gsl_block.h>
//#include <gsl/gsl_vector.h>
//#include <gsl/gsl_matrix.h>
//#include <gsl/gsl_blas.h>
#include "pdp.h"
#include "pdp_base.h"


typedef struct pdp_mat_mm_struct
{
    t_pdp_base x_base;
    CBLAS_TRANSPOSE_t x_T0;
    CBLAS_TRANSPOSE_t x_T1;
    int x_M0;
    int x_M1;

    float x_scale_r;
    float x_scale_i;

} t_pdp_mat_mm;


static void pdp_mat_mm_rscale(t_pdp_mat_mm *x, t_floatarg r)
{
    x->x_scale_r = r;
    x->x_scale_i = 0.0f;
}

static void pdp_mat_mm_cscale(t_pdp_mat_mm *x, t_floatarg r, t_floatarg i)
{
    x->x_scale_r = r;
    x->x_scale_i = i;
}


/* matrix multilpy */
static void pdp_mat_mv_process_mul(t_pdp_mat_mm *x)
{
    int pA = pdp_base_get_packet(x, 0);
    int pB = pdp_base_get_packet(x, 1);
    int p0, p1, pR;

    /* determine which one is the vector */
    if (pdp_packet_matrix_isvector(pA)){
	p0 = pB;
	p1 = pA;
    }
    else {
	p1 = pB;
	p0 = pA;
    }

    pR = pdp_packet_new_matrix_product_result(x->x_T0, CblasNoTrans, p0, p1);

    if (-1 != pR){
	pdp_packet_matrix_setzero(pR);
	if (pdp_packet_matrix_blas_mv(x->x_T0, p0, p1, pR, x->x_scale_r, x->x_scale_i)){
	    //post("pdp_packet_matrix_blas_mm failed");
	    pdp_packet_mark_unused(pR);
	    pR = -1;
	}
    }
    else {
	//post("pdp_packet_new_matrix_product_result failed");
    }

    /* replace with result */
    pdp_base_set_packet(x, 0, pR);

}

/* matrix vector multilpy */
static void pdp_mat_mm_process_mul(t_pdp_mat_mm *x)
{
    int pA = pdp_base_get_packet(x, 0);
    int pB = pdp_base_get_packet(x, 1);
    int p0, p1, pR;

    p0 = (x->x_M0) ? pB : pA;
    p1 = (x->x_M1) ? pB : pA;

    pR = pdp_packet_new_matrix_product_result(x->x_T0, x->x_T1, p0, p1);

    if (-1 != pR){
	pdp_packet_matrix_setzero(pR);
	if (pdp_packet_matrix_blas_mm(x->x_T0, x->x_T1, p0, p1, pR, x->x_scale_r, x->x_scale_i)){
	    //post("pdp_packet_matrix_blas_mm failed");
	    pdp_packet_mark_unused(pR);
	    pR = -1;
	}
    }
    else {
	//post("pdp_packet_new_matrix_product_result failed");
    }

    /* replace with result */
    pdp_base_set_packet(x, 0, pR);

}
/* matrix macc */
static void pdp_mat_mm_process_mac(t_pdp_mat_mm *x)
{
    int pC = pdp_base_get_packet(x, 0);
    int pA = pdp_base_get_packet(x, 1);
    int pB = pdp_base_get_packet(x, 2);
    int p0, p1;

    p0 = (x->x_M0) ? pB : pA;
    p1 = (x->x_M1) ? pB : pA;

    if (pdp_packet_matrix_blas_mm(x->x_T0, x->x_T1, p0, p1, pC, x->x_scale_r, x->x_scale_i)){
	//post("pdp_packet_matrix_blas_mm failed");
	pdp_base_set_packet(x, 0, -1); // delete packet
    }

}


static void pdp_mat_mm_free(t_pdp_mat_mm *x)
{
    /* remove process method from queue before deleting data */
    pdp_base_free(x);
}

t_class *pdp_mat_mm_class;


/* common new method */
void *pdp_mat_mm_new(void)
{
    int i;
    t_pdp_mat_mm *x = (t_pdp_mat_mm *)pd_new(pdp_mat_mm_class);
 
    /* super init */
    pdp_base_init(x);

    /* outlet */
    pdp_base_add_pdp_outlet(x);


    return (void *)x;
}


static int pdp_mat_mm_setup_routing_M0(t_pdp_mat_mm *x, t_symbol *s0)
{
     if ('A' == s0->s_name[0]){x->x_M0 = 0;} else if ('B' == s0->s_name[0]) {x->x_M0 = 1;} else return 0;

     if ((gensym("A") == s0) || (gensym("B") == s0)) x->x_T0 = CblasNoTrans;
     else if ((gensym("A^T") == s0) || (gensym("B^T") == s0)) x->x_T0 = CblasConjTrans;
     else if ((gensym("A^H") == s0) || (gensym("B^H") == s0)) x->x_T0 = CblasConjTrans;
     else return 0;

     return 1;
}

static int pdp_mat_mm_setup_routing_M1(t_pdp_mat_mm *x, t_symbol *s1)
{

     if ('A' == s1->s_name[0]){x->x_M1 = 0;} else if ('B' == s1->s_name[0]) {x->x_M1 = 1;} else return 0;

     /* setup second matrix transpose operation */
     if ((gensym("A") == s1) || (gensym("B") == s1)) x->x_T1 = CblasNoTrans;
     else if ((gensym("A^T") == s1) || (gensym("B^T") == s1)) x->x_T1 = CblasConjTrans;
     else if ((gensym("A^H") == s1) || (gensym("B^H") == s1)) x->x_T1 = CblasConjTrans;
     else return 0;

     return 1;
}


static int pdp_mat_mm_setup_scaling(t_pdp_mat_mm *x, t_symbol *scale)
{
    int success = 1;

     /* setup scaling inlet */
     if ((gensym ("rscale") == scale) || (gensym("r") == scale)){
	 pdp_base_add_gen_inlet(x, gensym("float"), gensym("rscale"));
     }
     else if ((gensym ("cscale") == scale) || (gensym("c") == scale)){
	 pdp_base_add_gen_inlet(x, gensym("list"), gensym("cscale"));
     }
     else if (gensym ("") != scale) success = 0;
     
     return success;
}

void *pdp_mat_mm_new_mul_common(t_symbol *s0, t_symbol *s1, t_symbol *scale, int ein)
{
     t_pdp_mat_mm *x = pdp_mat_mm_new();

     /* add extra pdp inlets */
     while (ein--) pdp_base_add_pdp_inlet(x);

     /* setup routing */
     if (!pdp_mat_mm_setup_routing_M0(x, s0)) goto error;
     if (!pdp_mat_mm_setup_routing_M1(x, s1)) goto error;
     if (!pdp_mat_mm_setup_scaling(x, scale)) goto error;

     /* default scale = 1 */
     pdp_mat_mm_cscale(x, 1.0f, 0.0f);
     return (void *)x;

 error:
     pd_free((void *)x);
     return 0;
}

void *pdp_mat_mv_new_mul_common(t_symbol *s0, t_symbol *scale, int ein)
{
     t_pdp_mat_mm *x = pdp_mat_mm_new();

     /* add extra pdp inlets */
     while (ein--) pdp_base_add_pdp_inlet(x);

     /* setup routing */
     if (!pdp_mat_mm_setup_routing_M0(x, s0)) goto error;
     if (!pdp_mat_mm_setup_scaling(x, scale)) goto error;

     /* default scale = 1 */
     pdp_mat_mm_cscale(x, 1.0f, 0.0f);
     return (void *)x;

 error:
     pd_free((void *)x);
     return 0;
}

void *pdp_mat_mm_new_mul(t_symbol *s0, t_symbol *s1, t_symbol *scale)
{
    t_pdp_mat_mm *x = pdp_mat_mm_new_mul_common(s0, s1, scale, 1);
    if(x){
	pdp_base_set_process_method(x, (t_pdp_method)pdp_mat_mm_process_mul);
	pdp_base_readonly_active_inlet(x);
    }
    return x;
}

void *pdp_mat_mv_new_mul(t_symbol *s0, t_symbol *scale)
{
    t_pdp_mat_mm *x = pdp_mat_mv_new_mul_common(s0, scale, 1);
    if(x){
	pdp_base_set_process_method(x, (t_pdp_method)pdp_mat_mv_process_mul);
	pdp_base_readonly_active_inlet(x);
    }
    return x;
}

void *pdp_mat_mm_new_mac(t_symbol *s0, t_symbol *s1, t_symbol *scale)
{
    t_pdp_mat_mm *x = pdp_mat_mm_new_mul_common(s0, s1, scale, 2);
    if (x){
	pdp_base_set_process_method(x, (t_pdp_method)pdp_mat_mm_process_mac);
    }
    return x;
}


#ifdef __cplusplus
extern "C"
{
#endif


void pdp_mat_mul_setup(void)
{


    pdp_mat_mm_class = class_new(gensym("pdp_m_mm"), (t_newmethod)pdp_mat_mm_new_mul,
    	(t_method)pdp_mat_mm_free, sizeof(t_pdp_mat_mm), 0, A_SYMBOL, A_SYMBOL, A_DEFSYMBOL, A_NULL);

    pdp_base_setup(pdp_mat_mm_class);

    class_addcreator((t_newmethod)pdp_mat_mm_new_mac, gensym("pdp_m_+=mm"),
		     A_SYMBOL, A_SYMBOL, A_DEFSYMBOL, A_NULL);
  
    class_addcreator((t_newmethod)pdp_mat_mv_new_mul, gensym("pdp_m_mv"),
		     A_SYMBOL, A_DEFSYMBOL, A_NULL);
  

    class_addmethod(pdp_mat_mm_class, (t_method)pdp_mat_mm_rscale, gensym("rscale"), A_FLOAT, A_NULL);
    class_addmethod(pdp_mat_mm_class, (t_method)pdp_mat_mm_cscale, gensym("cscale"), A_FLOAT, A_FLOAT, A_NULL);

}

#ifdef __cplusplus
}
#endif

--- NEW FILE: pdp_mat_vec.c ---
/*
 *   Pure Data Packet module. Vector modules.
 *   Copyright (c) by Tom Schouten <pdp at zzz.kotnet.org>
 *
 *   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.
 *
 */

//#include <gsl/gsl_block.h>
//#include <gsl/gsl_vector.h>
//#include <gsl/gsl_matrix.h>
//#include <gsl/gsl_blas.h>
#include "pdp.h"
#include "pdp_base.h"


typedef struct pdp_mat_vec_struct
{
    t_pdp_base x_base;
    int x_type;
    t_outlet *x_out;
    int x_accept_list;
    int x_list_size;
    t_atom *x_list;

} t_pdp_mat_vec;


#define GETFLOAT(x) ((x)->a_type == A_FLOAT ? (x)->a_w.w_float : 0.0f)
#define GETDOUBLE(x) (double)GETFLOAT(x)


static void pdp_mat_vec_list_in(t_pdp_mat_vec *x, t_symbol *s, int argc, t_atom *argv)
{
    int i;
    int vp = -1;
    int f;
    int dim = argc;

    if (!x->x_accept_list) return; //check if this handler is enabled
    if (!argc) return; //reject empty list

    switch(x->x_type){
    case PDP_MATRIX_TYPE_CFLOAT:
	if (argc & 1) return; //reject odd nb elements
	dim >>= 1; //halve dimension
    case PDP_MATRIX_TYPE_RFLOAT:
	vp = pdp_packet_new_matrix(dim, 1, x->x_type);
	if (-1 != vp){
	    float *data = (float *)pdp_packet_data(vp);
	    for (i=0; i<argc; i++) data[i] = GETFLOAT(&argv[i]);
	}
	break;
    case PDP_MATRIX_TYPE_CDOUBLE:
	if (argc & 1) return; //reject odd nb elements
	dim >>= 1; //halve dimension
    case PDP_MATRIX_TYPE_RDOUBLE:
	vp = pdp_packet_new_matrix(dim, 1, x->x_type);
	if (-1 != vp){
	    double *data = (double *)pdp_packet_data(vp);
	    for (i=0; i<argc; i++) data[i] = GETDOUBLE(&argv[i]);
	}
	break;
    default:
	break;
    }

    if (-1 != vp){
	/* store vector packet */
	pdp_base_set_packet(x, 0, vp);
	pdp_base_bang(x);
    }
}

static void pdp_mat_vec_list_out(t_pdp_mat_vec *x)
{
    int p = pdp_base_move_packet(x, 0);
    int type = pdp_packet_matrix_get_type(p);
    int outlist_size;
    float *fdata = 0;
    double *ddata = 0;
    int i;

    /* check if it's a vector */
    gsl_vector *m = (gsl_vector *)pdp_packet_matrix_get_gsl_vector(p, type);
    if (!pdp_packet_matrix_isvector(p)) return;

    /* get list size */
    outlist_size = m->size;
    if ((type == PDP_MATRIX_TYPE_CFLOAT)
	|| (type == PDP_MATRIX_TYPE_CDOUBLE)) 
	outlist_size <<= 1;
    
    /* realloc list if necessary */
    if (outlist_size > x->x_list_size){
	free(x->x_list);
	x->x_list = (t_atom *)pdp_alloc(sizeof(t_atom) * outlist_size);
	x->x_list_size = outlist_size;
    }

    /* copy data */
    switch(type){
    case PDP_MATRIX_TYPE_RFLOAT:
    case PDP_MATRIX_TYPE_CFLOAT:
	fdata = (float *)pdp_packet_data(p);
	for (i=0; i<outlist_size; i++)
	    SETFLOAT(&x->x_list[i], fdata[i]);
	break;
	
    case PDP_MATRIX_TYPE_RDOUBLE:
    case PDP_MATRIX_TYPE_CDOUBLE:
	ddata = (double *)pdp_packet_data(p);
	for (i=0; i<outlist_size; i++)
	    SETFLOAT(&x->x_list[i], (float)ddata[i]);
	break;
	
    }	
			    
    /* dispose of vector packet and output list */
    pdp_packet_mark_unused(p);
    outlet_list(x->x_out, &s_list, outlist_size, x->x_list);
}

static void pdp_mat_vec_free(t_pdp_mat_vec *x)
{
    /* remove process method from queue before deleting data */
    pdp_base_free(x);

    /* delete list */
    if (x->x_list) pdp_dealloc (x->x_list);
}

t_class *pdp_mat_vec_class;


/* common new methods */
t_pdp_mat_vec *pdp_mat_vec_base_new(void)
{
    t_pdp_mat_vec *x = (t_pdp_mat_vec *)pd_new(pdp_mat_vec_class);
    pdp_base_init(x);
    x->x_type = PDP_MATRIX_TYPE_CFLOAT;
    x->x_accept_list = 0;
    x->x_list_size = 0;
    x->x_list = 0;
    return x;
}

void *pdp_mat_vec_list2vec_new(t_symbol *type)
{
    t_pdp_mat_vec *x = pdp_mat_vec_base_new();
    pdp_base_disable_active_inlet(x);
    pdp_base_add_pdp_outlet(x);
    x->x_accept_list = 1;
    if ((gensym ("") == type) || (gensym ("double/real") == type)) x->x_type = PDP_MATRIX_TYPE_RDOUBLE;
    else if (gensym ("double/complex") == type) x->x_type = PDP_MATRIX_TYPE_CDOUBLE;
    else if (gensym ("float/real") == type) x->x_type = PDP_MATRIX_TYPE_RFLOAT;
    else if (gensym ("float/complex") == type) x->x_type = PDP_MATRIX_TYPE_CFLOAT;
    else {
	pd_free((t_pd *)x);
	x = 0;
    }
    return (void *)x;
}


void *pdp_mat_vec_vec2list_new(t_symbol *type)
{
    t_pdp_mat_vec *x = pdp_mat_vec_base_new();
    x->x_out = outlet_new((t_object *)x, &s_anything);
    pdp_base_set_postproc_method(x,(t_pdp_method)pdp_mat_vec_list_out);
    pdp_base_readonly_active_inlet(x);
    return (void *)x;
}


#ifdef __cplusplus
extern "C"
{
#endif


void pdp_mat_vec_setup(void)
{


    pdp_mat_vec_class = class_new(gensym("pdp_m_list2vec"), (t_newmethod)pdp_mat_vec_list2vec_new,
    	(t_method)pdp_mat_vec_free, sizeof(t_pdp_mat_vec), 0, A_DEFSYMBOL, A_NULL);

    pdp_base_setup(pdp_mat_vec_class);


    class_addcreator((t_newmethod)pdp_mat_vec_vec2list_new, gensym("pdp_m_vec2list"), A_NULL);

    class_addlist(pdp_mat_vec_class, (t_method)pdp_mat_vec_list_in);
 

}

#ifdef __cplusplus
}
#endif

--- NEW FILE: Makefile ---
current: all_modules

include ../../Makefile.config

PDP_MOD = $(PDP_MATRIX_BASIC)

all_modules: $(PDP_MOD)

clean:
	rm -f *~
	rm -f *.o


--- NEW FILE: pdp_mat_lu.c ---
/*
 *   Pure Data Packet module. LU decomposition module.
 *   Copyright (c) by Tom Schouten <pdp at zzz.kotnet.org>
 *
 *   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.
 *
 */

//#include <gsl/gsl_block.h>
//#include <gsl/gsl_vector.h>
//#include <gsl/gsl_matrix.h>
//#include <gsl/gsl_blas.h>
#include "pdp.h"
#include "pdp_base.h"


typedef struct pdp_mat_LU_struct
{
    t_pdp_base x_base;

} t_pdp_mat_LU;



static void pdp_mat_LU_process_LU_inverse(t_pdp_mat_LU *x)
{
    int p = pdp_base_get_packet(x, 0);
    int p_LU = pdp_packet_matrix_LU_to_inverse(p);
    pdp_base_set_packet(x, 0, p_LU); // replace packet
}

static void pdp_mat_LU_process_LU(t_pdp_mat_LU *x)
{
    int p = pdp_base_get_packet(x, 0);
    int p_LU = pdp_packet_matrix_LU(p);
    pdp_base_set_packet(x, 0, p_LU); // replace packet
}

static void pdp_mat_LU_process_LU_solve(t_pdp_mat_LU *x)
{
    int p0 = pdp_base_get_packet(x, 0);
    int p1 = pdp_base_get_packet(x, 1);
    int pvr, pm, pv;

    /* determine which is vector and which is matrix */
    if (pdp_packet_matrix_ismatrix(p0) && pdp_packet_matrix_isvector(p1)){
	pm = p0;
	pv = p1;
    }
    else {
	pm = p1;
	pv = p0;
    }

    /* create the result vector */
    pvr = pdp_packet_matrix_LU_solve(pm, pv);

    /* replace the active packet */
    pdp_base_set_packet(x, 0, pvr);
}

static void pdp_mat_LU_free(t_pdp_mat_LU *x)
{
    /* remove process method from queue before deleting data */
    pdp_base_free(x);
}

t_class *pdp_mat_LU_class;


/* common new methods */
t_pdp_mat_LU *pdp_mat_LU_base_new(void)
{
    t_pdp_mat_LU *x = (t_pdp_mat_LU *)pd_new(pdp_mat_LU_class);
    pdp_base_init(x);
    pdp_base_add_pdp_outlet(x);
    return x;
}

void *pdp_mat_LU_inverse_new(void)
{
    t_pdp_mat_LU *x = pdp_mat_LU_base_new();
    pdp_base_set_process_method(x,(t_pdp_method)pdp_mat_LU_process_LU_inverse);
    pdp_base_readonly_active_inlet(x);
    return (void *)x;
}


void *pdp_mat_LU_new(void)
{
    t_pdp_mat_LU *x = pdp_mat_LU_base_new();
    pdp_base_set_process_method(x,(t_pdp_method)pdp_mat_LU_process_LU);
    pdp_base_readonly_active_inlet(x);
    return (void *)x;
}

void *pdp_mat_LU_solve_new(void)
{
    t_pdp_mat_LU *x = pdp_mat_LU_base_new();
    pdp_base_set_process_method(x,(t_pdp_method)pdp_mat_LU_process_LU_solve);
    pdp_base_readonly_active_inlet(x);
    pdp_base_add_pdp_inlet(x);
    return (void *)x;
}

#ifdef __cplusplus
extern "C"
{
#endif


void pdp_mat_lu_setup(void)
{


    pdp_mat_LU_class = class_new(gensym("pdp_m_LU_inverse"), (t_newmethod)pdp_mat_LU_inverse_new,
    	(t_method)pdp_mat_LU_free, sizeof(t_pdp_mat_LU), 0, A_NULL);

    pdp_base_setup(pdp_mat_LU_class);


    class_addcreator((t_newmethod)pdp_mat_LU_new, gensym("pdp_m_LU"), A_NULL);
    class_addcreator((t_newmethod)pdp_mat_LU_solve_new, gensym("pdp_m_LU_solve"), A_NULL);

 

}

#ifdef __cplusplus
}
#endif

--- NEW FILE: README ---
This directory contains "normal" matrix packet processors, 
derived from the t_pdp_base class defined in pdp_base.h

Most modules are wrappers around Gnu Scientific Library (gsl) calls


--- NEW FILE: clusterstuff.c ---
/*
 *   Pure Data Packet module.
 *   Copyright (c) 2003 by Tom Schouten <pdp at zzz.kotnet.org>
 *
 *   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.
 *
 */



#include "pdp.h"
#include "pdp_base.h"
#include <math.h>

struct _pdp_histo;
typedef void (*t_histo_proc)(struct _pdp_histo *);


/* the cluster struct */
typedef struct _cluster
{
    float N;
    float cx;
    float cy;
} t_cluster;



typedef struct _pdp_histo
{
    t_object x_obj;
    t_int x_logN;
    t_symbol *x_array_sym;
    t_float x_scale;
    t_int x_debug;
    t_int x_sample_size;            /* pointcloud size */
    t_int x_nb_clusters;            /* nb of clusters */
    t_cluster *x_cluster;           /* cluster data (for tracking) */
    t_histo_proc x_process_method;  /* what to do with the histogram */
    t_outlet *x_outlet0;
    t_outlet *x_outlet1;

    /* the packet */
    int x_packet0;

    /* packet data */
    short int *x_data;
    int x_width;
    int x_height;
    int x_nb_pixels;

    /* histo data for processor: these are stored on the stack */
    int *x_histo;
    int *x_pixel_offset;
 
} t_pdp_histo;


// join 2 clusters. clear the second one
static void cluster_join(t_cluster *cA, t_cluster *cB)
{
    float scale = 1.0f / (cA->N + cB->N);
    cA->cx = (cA->N * cA->cx + cB->N * cB->cx) * scale;
    cA->cy = (cA->N * cA->cy + cB->N * cB->cy) * scale;
    cA->N += cB->N;

    cB->N = 0.0f;
}

static void cluster_copy(t_cluster *cA, t_cluster *cB)
{
    cA->cx = cB->cx;
    cA->cy = cB->cy;
    cA->N = cB->N;
}

static void cluster_clear(t_cluster *c)
{
    c->N = 0.0f;
}

static void cluster_new(t_cluster *c, float x, float y)
{
    c->N = 1.0f;
    c->cx = x;
    c->cy = y;
}

static float cluster_dsquared(t_cluster *cA, t_cluster *cB)
{
    float dx = cA->cx - cB->cx;
    float dy = cA->cy - cB->cy;
    return dx*dx + dy*dy;
}


static int round_up_2log(int i)
{
    int l = 0;
    i--;
    while (i) {
	i >>= 1;
	l++;
    }
    //post("log is %d, 2^n is %d", l, 1 << l);

    l = (l < 16) ? l : 15;
    return l;
}


static void compute_clusters(t_pdp_histo *x)
{
    t_cluster c[x->x_sample_size];
    int i;
    float scalex = 1.0f / (float)(x->x_width);
    float scaley = 1.0f / (float)(x->x_height);
    int nb_clusters = x->x_sample_size;

    /* build the cluster data struct */
    for (i=0; i<x->x_sample_size; i++)
	cluster_new(c+i, 
		    ((float)(x->x_pixel_offset[i] % x->x_width)) * scalex,
		    ((float)(x->x_pixel_offset[i] / x->x_width)) * scaley);

    /* the clustering loop */
    while (nb_clusters > x->x_nb_clusters){
	/* initialize cA, cB, d */
	int cA=0;
	int cB=1;
	float d = cluster_dsquared(c+0, c+1);
	int i,j;

	/* find the closest 2 clusters: 
	   scan the distance matrix above the diagonal */
	for (i=2; i<nb_clusters; i++){ 
	    for (j=0; j<i; j++){
		float dij = cluster_dsquared(c+i, c+j);
		if (dij < d){
		    cA = j;
		    cB = i;
		    d = dij;
		}
	    }
	}

	/* join the two clusters (cA < cB) */
	cluster_join (c+cA, c+cB);

	/* reduce the distance matrix by moving
	   the last element to the empty spot cB */
	nb_clusters--;
	cluster_copy (c+cB, c+nb_clusters);
    }

    /* copy cluster data */
    if (!x->x_cluster){
	int size =  sizeof(t_cluster) * x->x_nb_clusters;
	x->x_cluster = (t_cluster *)pdp_alloc(size);
	memcpy(x->x_cluster, c, size);
    }
    /* or perform tracking */
    else{
	int i,j;
	/* find best matches for the first couple of clusters */
	for (i=0; i<x->x_nb_clusters - 1; i++){
	    int closest = 0;
	    float d_min = cluster_dsquared(x->x_cluster+i, c);

	    /* get closest cluster */
	    for (j=1; j<nb_clusters; j++){
		float dj = cluster_dsquared(x->x_cluster+i, c+j);
		if (dj < d_min){
		    closest = j;
		    d_min = dj;
		} 
	    }

	    /* replace reference cluster with closest match */
	    cluster_copy(x->x_cluster+i, c+closest);

	    /* shrink matrix (like above) */
	    nb_clusters--;
	    cluster_copy(c+closest, c+nb_clusters);

	}
	/* copy the last cluster */
	cluster_copy(x->x_cluster + x->x_nb_clusters - 1, c);
    }

    /* print the clusters */
    post("clusters:");
    post("\tN\tcx\tcy");
    for (i=0; i<x->x_nb_clusters; i++){
	post("\t%d\t%0.2f\t%0.2f", 
	     (int)x->x_cluster[i].N, 
	     x->x_cluster[i].cx, 
	     x->x_cluster[i].cy);
    }



}

static void dump_to_array(t_pdp_histo *x)
{
    float *vec;
    int nbpoints;
    t_garray *a;
    int i;
    int *histo = x->x_histo;
    int N = 1 << (x->x_logN);
    float scale = 1.0f / (float)(x->x_nb_pixels);
    

    /* dump to array if possible */
    if (!x->x_array_sym){
    }

    /* check if array is valid */
    else if (!(a = (t_garray *)pd_findbyclass(x->x_array_sym, garray_class))){
        post("pdp_histo: %s: no such array", x->x_array_sym->s_name);
    }
    /* get data */
    else if (!garray_getfloatarray(a, &nbpoints, &vec)){
        post("pdp_histo: %s: bad template", x->x_array_sym->s_name);
    }
    /* scale and dump in array */
    else{

	N = (nbpoints < N) ? nbpoints : N;
	for (i=0; i<N; i++) vec[i] = (float)(histo[i]) * scale * x->x_scale;
	//garray_redraw(a);
    }

}

static void get_sampleset(t_pdp_histo *x, int log_tmp_size, int threshold)
{
    int N = 1 << log_tmp_size;
    int mask = N-1;
    int index, nbpoints, i;
    t_atom a[2];
    float scalex = 1.0f / (float)(x->x_width);
    float scaley = 1.0f / (float)(x->x_height);
    t_symbol *s = gensym("list");

    /* store the offsets of the points in a in an oversized array
       the oversizing is to eliminate a division and to limit the
       searching for a free location after a random index is generated */

    int offset[N]; 

    /* float versions of the coordinates */
    float fx[x->x_sample_size];
    float fy[x->x_sample_size];
    float max_x, min_x, max_y, min_y;

    /* reset the array */
    memset(offset, -1, N * sizeof(int));

    /* get the coordinates of the tempsize brightest points
       and store them in a random location in the hash */
    for (i=0; i<x->x_nb_pixels; i++){
	if (x->x_data[i] >= threshold){
	    /* get a random index */
	    int ri = random();
	    //int ri = 0;
	    /* find an empty spot to store it */
	    while (-1 != offset[ri & mask]) ri++;
	    offset[ri & mask] = i;
	}
    }


    /* repack the array to get the requested
       sample size at the start */
    index = 0;
    nbpoints = 0;
    while (nbpoints < x->x_sample_size){
	while (-1 == offset[index]) index++;   // ffwd to next nonepty slot
	offset[nbpoints++] = offset[index++];  // move slot
    }
	
    /* mark output packet samples */
    memset(x->x_data, 0, 2*x->x_nb_pixels);
    for (i=0; i<x->x_sample_size; i++){
	x->x_data[offset[i]] = 0x7fff;
    }

    /* send packet to left outlet */
    pdp_pass_if_valid(x->x_outlet0, &x->x_packet0);


    /* run the clustering algo */
    x->x_pixel_offset = offset;
    compute_clusters(x);


}

static void get_brightest(t_pdp_histo *x)
{
    int i;
    int *histo = x->x_histo;
    int N = 1 << (x->x_logN);

    int index, nsamps;

    /* check requested size */
    if (x->x_sample_size > x->x_nb_pixels){
	post("WARNING: more samples requested than pixels in image");
	x->x_sample_size = x->x_nb_pixels;
    }
    

    /* find limiting index */
    index = N;
    nsamps = 0;
    while (nsamps < x->x_sample_size){
	index--;
	nsamps += histo[index];
    }

    /* status report */
    if (x->x_debug){
	post("found %d samples between h[%d] and h[%d]", nsamps, index, N-1);
    }

    /* get a representative set from the candidates 
       the tempbuf is the rounded log of the nb of samples + 1
       so it is at least 50% sparse */
    get_sampleset(x, round_up_2log(nsamps) + 1, index << (15-x->x_logN));

}


static void _pdp_histo_perform(t_pdp_histo *x)
{
    short int *pp;
    int N = 1 << x->x_logN;
    int nbpixels = x->x_width * x->x_height, i;

    int histo[N];

    /* init */
    for (i=0; i<N; i++) histo[i] = 0;

    /* build histo */
    for (i=0; i<nbpixels; i++){
	int index = x->x_data[i] >> (15 - x->x_logN);
	if (index < 0) index = 0; /* negative -> zero */
	histo[index]++;
    }

    /* save the histo stack location  */
    x->x_histo = histo;

    /* print it */
    if (x->x_debug){
	post("histogram:");
	for (i=0; i<N; i++){
	    fprintf(stderr, "%d\t", histo[i]);
	    if (!(i % 10)) post("");
	}
	post("");
    }

    /* call the processor */
    x->x_process_method(x);


}
  

// packet is an image/*/* packet or invalid */
static void pdp_histo_perform(t_pdp_histo *x)
{
    t_pdp *header0 = pdp_packet_header(x->x_packet0);
    void *data0 = pdp_packet_data(x->x_packet0);
    if (!header0 || !data0) return;

    x->x_width = header0->info.image.width;
    x->x_height = header0->info.image.height;
    x->x_nb_pixels = x->x_width * x->x_height;
    x->x_data = data0;

    _pdp_histo_perform(x);
}



static void pdp_histo_input_0(t_pdp_histo *x, t_symbol *s, t_floatarg f)
{
    int packet = (int)f;

    /* register */
    if (s == gensym("register_ro")){
	/* replace if not compatible or we are not interpolating */
	pdp_packet_mark_unused(x->x_packet0);
	x->x_packet0 = pdp_packet_convert_rw(packet, pdp_gensym("image/grey/*"));
	
    }

    if (s == gensym("process")){
	pdp_histo_perform(x);
    }

}



static void pdp_histo_samplesize(t_pdp_histo *x, t_floatarg f)
{
    int i = (int)f;
    if (i >= x->x_nb_clusters ) x->x_sample_size = i;
}

static void pdp_histo_clusters(t_pdp_histo *x, t_floatarg f)
{
    int i = (int)f;
    if (i>=2 && i<= x->x_sample_size){
	x->x_nb_clusters = i;
	if (x->x_cluster) pdp_dealloc(x->x_cluster);
	x->x_cluster = 0;
    }
}
static void pdp_histo_scale(t_pdp_histo *x, t_floatarg f){x->x_scale = f;}



static void pdp_histo_size(t_pdp_histo *x, t_floatarg f)
{
    int i = (int)f;
    if (i < 1) return;
    x->x_logN = round_up_2log(i);
}


static void pdp_histo_array(t_pdp_histo *x, t_symbol *s)
{
    //post("setting symbol %x", s);
    x->x_array_sym = s;
}


static void pdp_histo_free(t_pdp_histo *x)
{
    pdp_packet_mark_unused(x->x_packet0);
    if (x->x_cluster) pdp_dealloc(x->x_cluster);
}


t_class *pdp_histo_class;



void *pdp_histo_new(t_floatarg f)
{

    t_pdp_histo *x = (t_pdp_histo *)pd_new(pdp_histo_class);
    if (f == 0.0f) f = 64;
    pdp_histo_size(x, f);
    x->x_packet0 = -1;
    x->x_debug = 0;
    x->x_sample_size = 16;
    x->x_nb_clusters = 3;
    x->x_cluster = 0;
    return (void *)x;
}


void *pdp_histo_array_new(t_symbol *s, t_float f, t_float f2)
{
    t_pdp_histo *x = (t_pdp_histo *)pdp_histo_new(f);
    if (f2 == 0.0f) f2 = 1.0f;
    pdp_histo_scale(x, f2);
    pdp_histo_array(x, s);
    x->x_process_method = dump_to_array;
    return (void *)x;
}    

void *pdp_histo_sample_new(t_float nbsamples, t_float histosize)
{
    t_pdp_histo *x;
    if (histosize == 0.0f) histosize = 256.0f;
    x = (t_pdp_histo *)pdp_histo_new(histosize);
    if (nbsamples == 0.0f) nbsamples = 16.0f;
    pdp_histo_samplesize(x, nbsamples);
    x->x_process_method = get_brightest;
    x->x_outlet0 = outlet_new(&x->x_obj, gensym("anything"));
    //x->x_outlet1 = outlet_new(&x->x_obj, gensym("anything"));

    inlet_new((t_object *)x, (t_pd *)&x->x_obj, gensym("float"), gensym("nbpoints"));

    return (void *)x;
}    


#ifdef __cplusplus
extern "C"
{
#endif


void pdp_histo_setup(void)
{

    pdp_histo_class = class_new(gensym("pdp_histo"), (t_newmethod)pdp_histo_array_new,
    	(t_method)pdp_histo_free, sizeof(t_pdp_histo), 0, A_DEFSYMBOL, A_DEFFLOAT, A_DEFFLOAT, A_NULL);

    class_addcreator((t_newmethod)pdp_histo_sample_new, gensym("pdp_pointcloud"), A_DEFFLOAT, A_DEFFLOAT, A_NULL);
     
    class_addmethod(pdp_histo_class, (t_method)pdp_histo_input_0, gensym("pdp"),  A_SYMBOL, A_DEFFLOAT, A_NULL);

    class_addmethod(pdp_histo_class, (t_method)pdp_histo_size, gensym("size"), A_FLOAT, A_NULL);
    class_addmethod(pdp_histo_class, (t_method)pdp_histo_size, gensym("scale"), A_FLOAT, A_NULL);
    class_addmethod(pdp_histo_class, (t_method)pdp_histo_array, gensym("array"), A_SYMBOL, A_NULL);
    class_addmethod(pdp_histo_class, (t_method)pdp_histo_samplesize, gensym("nbpoints"), A_FLOAT, A_NULL);
    class_addmethod(pdp_histo_class, (t_method)pdp_histo_clusters, gensym("nbclusters"), A_FLOAT, A_NULL);
}

#ifdef __cplusplus
}
#endif