#include "winary_t.h"


#define sWINARY_ADD ("add")
#define sWINARY_FILTER ("filter")
#define sWINARY_THRESHOLD ("threshold")
#define sWINARY_SHIFT ("shift")


void winary_t_setup(void){
    pcWinary_t = class_new(gensym("winary~"),
          (t_newmethod)winary_t_new, (t_method)winary_t_delete,
          sizeof(sWinary_t),
          CLASS_DEFAULT,
          A_DEFSYMBOL, A_DEFFLOAT, A_DEFFLOAT, A_DEFFLOAT, 0);
    class_addmethod(pcWinary_t, (t_method)winary_t_dsp, gensym("dsp"), 0);
    CLASS_MAINSIGNALIN(pcWinary_t, sWinary_t, const_sig);
    class_addbang(pcWinary_t, winary_t_bang);
    
    class_addmethod(pcWinary_t, (t_method)winary_t_set_float_param, gensym("set_float_param"), A_DEFFLOAT, A_DEFFLOAT,0);
    class_addmethod(pcWinary_t, (t_method)winary_t_set_int_param, gensym("set_int_param"), A_DEFFLOAT, A_DEFFLOAT,0);
}
void winary_t_set_pointers_to_NULL(sWinary_t *psW){
    psW->a_in = NULL;
    psW->a_out = NULL;
    psW->pawc=NULL;
    return;
}
void  *winary_t_new(t_symbol *whichone, t_float val0, t_float val1, t_float val2){
    
    sWinary_t *psW=(sWinary_t *) pd_new(pcWinary_t);
  
    winary_t_set_pointers_to_NULL(psW);
    
    winary_t_initialize(psW); //Alles loeschen
    
    if(whichone==gensym(sWINARY_ADD)) {
        psW->type=eWINARY_ADD;
        
       //Make 2 Inlets and Outlets
        psW->num_inlet=4;
        psW->num_outlet=2;

        psW->a_in=wabor_t_malloc0(psW->num_inlet * sizeof *(psW->a_in));
        psW->a_out=wabor_t_malloc0(psW->num_outlet * sizeof *(psW->a_out));

        //psW->a_in[0] //default inlet G1
        psW->a_in[1] = floatinlet_new(&psW->x_obj, &(psW->float_param[0])); 
        psW->a_in[2] = inlet_new(&(psW->x_obj), &(psW->x_obj.ob_pd), &s_signal, &s_signal); //in_G1
        psW->a_in[3] = floatinlet_new(&psW->x_obj, &(psW->float_param[1]));             

        psW->a_out[0] = outlet_new(&(psW->x_obj), &s_signal); //out_G
        psW->a_out[1] = outlet_new(&(psW->x_obj), &s_float);

    } else if (whichone==gensym(sWINARY_FILTER)) {        
        psW->type=eWINARY_FILTER;
        psW->float_param[0]=val0;
        psW->float_param[1]=val1;
        
        //Make 1 Inlet and 1 Outlet
        psW->num_inlet=3;
        psW->num_outlet=2;

        psW->a_in=wabor_t_malloc0(psW->num_inlet* sizeof *(psW->a_in));
        psW->a_out=wabor_t_malloc0(psW->num_outlet*sizeof *(psW->a_out));

        //psW->a_in[0] //default inlet G1
        psW->a_in[1] = floatinlet_new(&psW->x_obj, &(psW->float_param[0])); 
        psW->a_in[2] = floatinlet_new(&psW->x_obj, &(psW->float_param[1])); 
        psW->a_out[0] = outlet_new(&(psW->x_obj), &s_signal); //out_G
        psW->a_out[1] = outlet_new(&(psW->x_obj), &s_float);



    }  else if(whichone==gensym(sWINARY_THRESHOLD)) {
        psW->type=eWINARY_THRESHOLD;

        psW->float_param[0]=val0;
        
        psW->num_inlet=2;
        psW->num_outlet=2;
        psW->a_in=wabor_t_malloc0(psW->num_inlet* sizeof *(psW->a_in));
        psW->a_out=wabor_t_malloc0(psW->num_outlet*sizeof *(psW->a_out));
        //psW->a_in[0] //default inlet G1
        psW->a_in[1] = floatinlet_new(&psW->x_obj, &(psW->float_param[0])); 
        psW->a_out[0] = outlet_new(&(psW->x_obj), &s_signal); //out_G
        psW->a_out[1] = outlet_new(&(psW->x_obj), &s_float); 

    } else if(whichone==gensym(sWINARY_SHIFT)) {
        psW->type=eWINARY_SHIFT;

        psW->num_inlet=7;
        psW->num_outlet=2;

        psW->a_in=wabor_t_malloc0(psW->num_inlet* sizeof *(psW->a_in));
        psW->a_out=wabor_t_malloc0(psW->num_outlet*sizeof *(psW->a_out));
        //psW->a_in[0] //default inlet G1
        psW->a_in[1] = inlet_new(&(psW->x_obj), &(psW->x_obj.ob_pd), &s_signal, &s_bang); //C1_in
        psW->a_in[2] = floatinlet_new(&psW->x_obj, &(psW->float_param[0]));  //G2_in
        psW->a_in[3] = inlet_new(&(psW->x_obj), &(psW->x_obj.ob_pd), &s_signal, &s_signal); //C2_in
        psW->a_in[4] = floatinlet_new(&psW->x_obj, &(psW->float_param[0])); 
        psW->a_in[5] = floatinlet_new(&psW->x_obj, &(psW->float_param[1])); 
        psW->a_in[6] = floatinlet_new(&psW->x_obj, &(psW->float_param[2])); 
        psW->a_out[0] = outlet_new(&(psW->x_obj), &s_signal); //out_G
        psW->a_out[1] = outlet_new(&(psW->x_obj), &s_float); 
        
        psW->float_param[0]=val0; //XX
        psW->float_param[1]=val1;
        psW->float_param[2]=val2;

    } else {
        goto no_right_option;
    }     
    return (void *)psW;   
   
no_right_option: //if non of the above was chosen
    post("Winary Usage: See Help Patch");
    psW->num_inlet=0;
    psW->num_outlet=0;       
    psW->a_in=NULL;
    psW->a_out=NULL;
    return (void *)psW;   
}
void   winary_t_initialize( sWinary_t *psW){
    if(psW->a_in){
        for(t_int i=1;i<psW->num_inlet;++i){ //das nullte gibts es nicht, weil es durch das standard inlet ersetzt wird
            if(psW->a_in[i]) { 
                inlet_free(psW->a_in[i]);
                psW->a_in[i]=NULL;
            }
        }
        wabor_t_free(psW->a_in);
        psW->a_in=NULL;
    }    
    if(psW->a_out){
        for(t_int i=0;i<psW->num_outlet;++i){
            if(psW->a_out[i]){
                outlet_free(psW->a_out[i]);
                psW->a_out[i]=NULL;
            }
        }
        wabor_t_free(psW->a_out);
        psW->a_out=NULL;
    }
    if(psW->pawc) {
        wabor_t_free(psW->pawc); 
        psW->pawc=NULL;   
    }
}

void   winary_t_delete(sWinary_t *psW){
    winary_t_initialize(psW);
}

void   winary_t_dsp(sWinary_t *psW, t_signal **sp){
    
    if(psW->type==eWINARY_ADD){
        psW->dsp_num_pointers = 4;
        dsp_add(winary_t_perform_add, psW->dsp_num_pointers,
            psW,
            sp[0]->s_vec, //G1
            sp[1]->s_vec, //G2
            sp[2]->s_vec  //outG
            );
    } else if(psW->type==eWINARY_FILTER){
        psW->dsp_num_pointers = 3;
        dsp_add(winary_t_perform_filter, psW->dsp_num_pointers,
            psW,
            sp[0]->s_vec, //inG
            sp[1]->s_vec //outG
            );    
    } else if(psW->type==eWINARY_THRESHOLD){
        psW->dsp_num_pointers = 3;
        dsp_add(winary_t_perform_threshold, psW->dsp_num_pointers,
            psW,
            sp[0]->s_vec, //inG
            sp[1]->s_vec  //outG
            );    
    } else if(psW->type==eWINARY_SHIFT){
        psW->int_param[0]=0; //counter for samples
        psW->dsp_num_pointers = 5;
        dsp_add(winary_t_perform_shift, psW->dsp_num_pointers,
            psW,
            sp[0]->s_vec, //G1
            sp[1]->s_vec, //C1
            //sp[X]->s_vec, //G2 , is designed as floatinlet, hence it gets no number
            sp[2]->s_vec, //C2
            sp[3]->s_vec  //OutG
            );    
    } 
}

t_int *winary_t_perform_add(t_int *aw){
    sWinary_t    *psW = (sWinary_t*)(aw[1]);
    t_sample     *inG1  = (t_sample *)(aw[2]);
    t_sample     *inG2  = (t_sample *)(aw[3]);  
    t_sample     *outG  = (t_sample *)(aw[4]);    
    
    sWabor_Coeff **pawc1,**pawc2;
    t_int num1,num2;
    if(   find_int_at_address(&inG1[WABOR_T_MAGIC_NUMBER_BIN]) != WABOR_T_MAGIC_NUMBER_G) {
        num1=0;
        pawc1=NULL;
    }else{
        pawc1 = find_pointer_at_address(&inG1[WABOR_T_POINTER_TO_COEFF_BIN]);
        num1=find_int_at_address(&inG1[WABOR_T_NUM_BLOCKS_BIN]);
    }
    
    if(   find_int_at_address(&inG2[WABOR_T_MAGIC_NUMBER_BIN]) != WABOR_T_MAGIC_NUMBER_G) {
        num2=0;
        pawc2=NULL;
    }else{
        pawc2 = find_pointer_at_address(&inG2[WABOR_T_POINTER_TO_COEFF_BIN]);
        num2=find_int_at_address(&inG2[WABOR_T_NUM_BLOCKS_BIN]);
    }
    
    t_int num=num1+num2;
    
    wabor_t_free(psW->pawc);
    
    psW->pawc=wabor_t_malloc0(num * sizeof *(psW->pawc));
    if(psW->pawc==NULL){
        //memset(&outG, 0, PD_BLOCKSIZE * sizeof *outG);
        memset(outG, 0, PD_BLOCKSIZE * sizeof *outG);
        return (aw + psW->dsp_num_pointers + 1);
    }
    
    for(t_int i=0; i<num1;++i){
        psW->pawc[i]=pawc1[i];
    }
    for(t_int i=num1;i<num;++i){
        psW->pawc[i]=pawc2[i-num1];
    }      
    hide_int_at_address( num, &outG[WABOR_T_NUM_BLOCKS_BIN]);
    hide_int_at_address( WABOR_T_MAGIC_NUMBER_G,        &outG[WABOR_T_MAGIC_NUMBER_BIN]);
    hide_pointer_at_address(psW->pawc,                  &outG[WABOR_T_POINTER_TO_COEFF_BIN]);
    
    
    return (aw + psW->dsp_num_pointers + 1);
    
}


t_int *winary_t_perform_filter(t_int *aw){
    sWinary_t    *psW = (sWinary_t*)(aw[1]);
    t_sample     *inG  = (t_sample *)(aw[2]);
    t_sample     *outG  = (t_sample *)(aw[3]);
    
    if(   find_int_at_address(&inG[WABOR_T_MAGIC_NUMBER_BIN]) != WABOR_T_MAGIC_NUMBER_G)
    {
        memset(outG,0,PD_BLOCKSIZE* sizeof *outG);
        return (aw + psW->dsp_num_pointers + 1);
    }
    sWabor_Coeff **patc = find_pointer_at_address(&inG[WABOR_T_POINTER_TO_COEFF_BIN]);
    
    t_int num=find_int_at_address(&inG[WABOR_T_NUM_BLOCKS_BIN]);
        
    t_int lower=psW->float_param[0];
    t_int upper=psW->float_param[1];
    for(t_int i=0; i<num;++i){
        t_int length=patc[i][0].numCoeff;
        for(t_int j=0; j<length; ++j){
            if(patc[i][j].band>=lower && patc[i][j].band<upper){
                patc[i][j].val_i=0;
                patc[i][j].val_r=0;
            }
        }
    }
    
    hide_int_at_address( num, &outG[WABOR_T_NUM_BLOCKS_BIN]);
    hide_int_at_address( WABOR_T_MAGIC_NUMBER_G,        &outG[WABOR_T_MAGIC_NUMBER_BIN]);
    hide_pointer_at_address(patc,                  &outG[WABOR_T_POINTER_TO_COEFF_BIN]);    
    
    return (aw + psW->dsp_num_pointers + 1);
    
}

t_int *winary_t_perform_threshold(t_int *aw){
    sWinary_t    *psW = (sWinary_t*)(aw[1]);
    t_sample     *inG  = (t_sample *)(aw[2]);
    t_sample     *outG  = (t_sample *)(aw[3]);
    
    if(   find_int_at_address(&inG[WABOR_T_MAGIC_NUMBER_BIN]) != WABOR_T_MAGIC_NUMBER_G)
    {
        memset(outG,0,PD_BLOCKSIZE* sizeof *outG);
        return (aw + psW->dsp_num_pointers + 1);
    }
    sWabor_Coeff **pawc = find_pointer_at_address(&inG[WABOR_T_POINTER_TO_COEFF_BIN]);
    
    t_int num=find_int_at_address(&inG[WABOR_T_NUM_BLOCKS_BIN]);

    tc_type thresh=psW->float_param[0];
    for(t_int i=0; i<num;++i){
        t_int length=pawc[i][0].numCoeff;
        for(t_int j=0; j<length; ++j){
            if(fabsPAC(pawc[i][j].val_r)<thresh) {
                pawc[i][j].val_r=0;
            }
            if(fabsPAC(pawc[i][j].val_i)<thresh) {
                pawc[i][j].val_i=0;
            }
        }
    }
    
    hide_int_at_address( num, &outG[WABOR_T_NUM_BLOCKS_BIN]);
    hide_int_at_address( WABOR_T_MAGIC_NUMBER_G,        &outG[WABOR_T_MAGIC_NUMBER_BIN]);
    hide_pointer_at_address(pawc,                  &outG[WABOR_T_POINTER_TO_COEFF_BIN]);    
    
    return (aw + psW->dsp_num_pointers + 1);
    
}

t_int *winary_t_perform_shift(t_int *aw){
    sWinary_t    *psW = (sWinary_t*)(aw[1]);
    t_sample     *inG1  = (t_sample *)(aw[2]);
    t_sample     *inC1  = (t_sample *)(aw[3]); 
    //t_sample   *inG2  = (t_sample *)(aw[X]);  //UNUSED // see above
    t_sample     *inC2  = (t_sample *)(aw[4]); 
    t_sample     *outG  = (t_sample *)(aw[5]);
    
    sWatom_Param *param1=NULL, *param2=NULL;
    sWabor_Coeff **pawc1=NULL;
    
    if(     find_int_at_address(&(inG1[WABOR_T_MAGIC_NUMBER_BIN])) != WABOR_T_MAGIC_NUMBER_G || 
            find_int_at_address(&(inC1[WABOR_T_MAGIC_NUMBER_BIN])) != WABOR_T_MAGIC_NUMBER_C ||
            find_int_at_address(&(inC2[WABOR_T_MAGIC_NUMBER_BIN])) != WABOR_T_MAGIC_NUMBER_C ||
            (param1=find_pointer_at_address(&(inC1[WABOR_T_WATOM_PARAM])))==NULL ||
            (param2=find_pointer_at_address(&(inC2[WABOR_T_WATOM_PARAM])))==NULL ||
            (pawc1 = find_pointer_at_address(&(inG1[WABOR_T_POINTER_TO_COEFF_BIN])))==NULL
      )
    {
        memset(outG,0,PD_BLOCKSIZE* sizeof *outG);
        return (aw + psW->dsp_num_pointers + 1);
    }
    
    paf_type **paf1_r=param1->paf_r;
    paf_type **paf2_r=param1->paf_r;
    t_int q2_sup=param2->iq_compute_sup;
    
    
    t_int num_blocks=find_int_at_address(&(inG1[WABOR_T_NUM_BLOCKS_BIN])); //number of blocks inside of pawc

    t_int shift=(int)psW->float_param[0];
    t_int shift_new=shift;
    for(t_int i=0; i<num_blocks;++i){
        t_int length_of_block=pawc1[i][0].numCoeff;
        for(t_int j=0; j<length_of_block; ++j){
            t_int q1=pawc1[i][j].band;
            t_int q2=q1+shift;
            if(q2<0) {
                q2=0;
                shift_new=-q1;
            }
            if(q2>=q2_sup){
                do{
                    q2=q2_sup-1;
                } while(q2>=q2_sup);
                shift_new=q2_sup-q1-1;
            }

             //starttime of original window
            t_int start1=pawc1[i][j].start;
            t_int wl1=paf1_r[q1][WABOR_T_LENGTH_BIN];
            t_int wl2=paf2_r[q2][WABOR_T_LENGTH_BIN];
            if(psW->float_param[1]>0){ 
                t_int timeshift=(wl1-wl2)/2;
                start1-=timeshift;
                pawc1[i][j].start=start1; 
            }
            if(psW->float_param[2]>0){ //XX Sollte phase shift nicht mit neuem Startzeitpunkt berechnet werden?
                
                t_int t0=psW->int_param[0]+start1;
                pac_type_real tc1=t0+wl1/2.0;
                pac_type_real tc2=t0+wl2/2.0;
                pac_type_real lambda1=1.0/(param1->Theta(param1->_b*q1,param1->thetadata));
                pac_type_real lambda2=1.0/(param2->Theta(param2->_b*q2,param2->thetadata)); //XX Hier fehlt Aufruf von Theta. Das heisst wenn q=0, kommt hier inf raus.
               pac_type_real tbar1,tbar2;
                if(lambda1==INFINITY) {
                    tbar1=0;
                } else {
                    tbar1=tc1-floorPAC(tc1/lambda1)*lambda1; 
                }
                if(lambda2==INFINITY) {
                    tbar2=0;
                } else {
                    tbar2=tc2-floorPAC(tc2/lambda2)*lambda2; 
                }
                
                pac_type_real delta_tbar=tbar1-tbar2; 
                pac_type_real delta_theta;
                if(lambda2==INFINITY) {
                    delta_theta=0;
                } else {
                    delta_theta=delta_tbar/lambda2*2*PI; 
                }
                
                pac_type val1=pawc1[i][j].val_r+I*pawc1[i][j].val_i;                
                pac_type_real theta2=cargPAC(val1)-delta_theta; //theta2=theta1-delta_theta //Das ist hier eigentlich theta2
                
                pac_type val2=cabsPAC(val1)*1*cexpPAC(I*theta2); //Das ist hier eigentlich val2, 1=Amplitudenfaktor a
                
                pawc1[i][j].val_r=(tc_type) crealPAC(val2);
                pawc1[i][j].val_i=(tc_type) cimagPAC(val2);  

            }
            pawc1[i][j].band=q2;
        }
    }
    
    hide_int_at_address( num_blocks, &outG[WABOR_T_NUM_BLOCKS_BIN]);
    hide_int_at_address( WABOR_T_MAGIC_NUMBER_G, &outG[WABOR_T_MAGIC_NUMBER_BIN]);
    hide_pointer_at_address(pawc1, &outG[WABOR_T_POINTER_TO_COEFF_BIN]);    
    
    psW->int_param[0]+=PD_BLOCKSIZE;
    return (aw + psW->dsp_num_pointers + 1);
    
}

void   winary_t_bang(sWinary_t *psW){
    post("Winary Buum Chzakk");
    post("Type: %i", psW->type);
    for(int i=0; i<WINARY_T_MAXPARAM; ++i){
        post("float_param[%i]=\t%f",i,psW->float_param[i]);
    }
    for(int i=0; i<WINARY_T_MAXPARAM; ++i){
        post("int_param[%i]=\t%i",i,(int)psW->int_param[i]);
    }        
}

void winary_t_set_float_param(sWinary_t *psW, t_float index, t_float val){
    if(index>=0 && index<WINARY_T_MAXPARAM){
        psW->float_param[(int)index]=val;
    }
}
void winary_t_set_int_param(sWinary_t *psW, t_float index, t_float val){
    if(index>=0 && index<WINARY_T_MAXPARAM){
        psW->int_param[(int)index]=(t_int)val;
    }    
}