//===============================================================================
// Copyright (c) 2007-2016  Advanced Micro Devices, Inc. All rights reserved.
// Copyright (c) 2004-2006 ATI Technologies Inc.
//===============================================================================
//
// 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.
//
// 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 NONINFRINGEMENT.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 <assert.h>
#include <math.h>
#include <float.h>

#include "3dquant_constants.h"
#include "3dquant_vpc.h"
#include "shake.h"
#include "bc7_utils.h"
#include "debug.h"
#include "bc7_encode.h"

#define LOG_CL_BASE 2
#define BIT_BASE 5

#define LOG_CL_RANGE 5
#define BIT_RANGE 9

#define CLT(cl)   (cl-LOG_CL_BASE)
#define BTT(bits) (bits-BIT_BASE)

int npv_nd[][2*MAX_DIMENSION_BIG]= {
    {}, // 0 - for alingment
    {}, // 1
    {}, // 2
    {1,2,4,8,16,32}, //3
    {1,2,4}          //4
};

// Reworking these tables
// Since endpoints can be flipped for the encoding (reclaims an index bit) this means
// that the parity tables must be symmetric for the two endpoints, as parity could also be flipped
// The tables above do not guarantee this behaviour, but this one does

int par_vectors_nd[][2*MAX_DIMENSION_BIG][(1<<(2*MAX_DIMENSION_BIG-1))][2][MAX_DIMENSION_BIG] = {
    {{{}}}, // 0 - for alingment

    {{{}}}, //1D

    {{{}}}, //2D

    {
        {{}},

// 3*n+1    BCC          3*n+1        Cartesian 3*n            //same parity
        {
            // SAME_PAR
            {{0,0,0},{0,0,0}},
            {{1,1,1},{1,1,1}}
        },
// 3*n+2    BCC          3*n+1        BCC          3*n+1
        {
            // BCC
            {{0,0,0},{0,0,0}},
            {{0,0,0},{1,1,1}},
            {{1,1,1},{0,0,0}},
            {{1,1,1},{1,1,1}}
        },
// 3*n+3    FCC                    ???                        // ??????
// BCC with FCC same or inverted, symmetric
        {
            // BCC_SAME_FCC
            {{0,0,0},{0,0,0}},
            {{1,1,0},{1,1,0}},
            {{1,0,1},{1,0,1}},
            {{0,1,1},{0,1,1}},

            {{0,0,0},{1,1,1}},
            {{1,1,1},{0,0,0}},
            {{0,1,0},{0,1,0}},  // ??
            {{1,1,1},{1,1,1}},

        },
// 3*n+4    FCC          3*n+2        FCC          3*n+2
        {

            {{0,0,0},{0,0,0}},
            {{1,1,0},{0,0,0}},
            {{1,0,1},{0,0,0}},
            {{0,1,1},{0,0,0}},

            {{0,0,0},{1,1,0}},
            {{1,1,0},{1,1,0}},
            {{1,0,1},{1,1,0}},
            {{0,1,1},{1,1,0}},

            {{0,0,0},{1,0,1}},
            {{1,1,0},{1,0,1}},
            {{1,0,1},{1,0,1}},
            {{0,1,1},{1,0,1}},

            {{0,0,0},{0,1,1}},
            {{1,1,0},{0,1,1}},
            {{1,0,1},{0,1,1}},
            {{0,1,1},{0,1,1}}
        },


// 3*n+5    Cartesian 3*n+3        FCC          3*n+2            //D^*[6]
        {

            {{0,0,0},{0,0,0}},
            {{1,1,0},{0,0,0}},
            {{1,0,1},{0,0,0}},
            {{0,1,1},{0,0,0}},

            {{0,0,0},{1,1,0}},
            {{1,1,0},{1,1,0}},
            {{1,0,1},{1,1,0}},
            {{0,1,1},{1,1,0}},

            {{0,0,0},{1,0,1}},
            {{1,1,0},{1,0,1}},
            {{1,0,1},{1,0,1}},
            {{0,1,1},{1,0,1}},

            {{0,0,0},{0,1,1}},
            {{1,1,0},{0,1,1}},
            {{1,0,1},{0,1,1}},
            {{0,1,1},{0,1,1}},


            {{1,0,0},{1,1,1}},
            {{0,1,0},{1,1,1}},
            {{0,0,1},{1,1,1}},
            {{1,1,1},{1,1,1}},

            {{1,0,0},{0,0,1}},
            {{0,1,0},{0,0,1}},
            {{0,0,1},{0,0,1}},
            {{1,1,1},{0,0,1}},

            {{1,0,0},{1,0,0}},
            {{0,1,0},{1,0,0}},
            {{0,0,1},{1,0,0}},
            {{1,1,1},{1,0,0}},

            {{1,0,0},{0,1,0}},
            {{0,1,0},{0,1,0}},
            {{0,0,1},{0,1,0}},
            {{1,1,1},{0,1,0}}
        }
    },

    {
        {{}},

// 3*n+1    BCC          3*n+1        Cartesian 3*n            //same parity
        {
            // SAME_PAR
            {{0,0,0,0},{0,0,0,0}},
            {{1,1,1,1},{1,1,1,1}}
        },
// 3*n+2    BCC          3*n+1        BCC          3*n+1
        {
            // BCC
            {{0,0,0,0},{0,0,0,0}},
            {{0,0,0,0},{1,1,1,1}},
            {{1,1,1,1},{0,0,0,0}},
            {{1,1,1,1},{1,1,1,1}}
        },
// 3 PBIT
        {
            {{0,0,0,0},{0,0,0,0}},
            {{0,0,0,0},{0,1,1,1}},
            {{0,1,1,1},{0,0,0,0}},
            {{0,1,1,1},{0,1,1,1}},

            {{1,0,0,0},{1,0,0,0}},
            {{1,0,0,0},{1,1,1,1}},
            {{1,1,1,1},{1,0,0,0}},
            {{1,1,1,1},{1,1,1,1}}
        },

// 4 PBIT
        {
            {{0,0,0,0},{0,0,0,0}},
            {{0,0,0,0},{0,1,1,1}},
            {{0,1,1,1},{0,0,0,0}},
            {{0,1,1,1},{0,1,1,1}},

            {{1,0,0,0},{1,0,0,0}},
            {{1,0,0,0},{1,1,1,1}},
            {{1,1,1,1},{1,0,0,0}},
            {{1,1,1,1},{1,1,1,1}},

            {{0,0,0,0},{0,0,0,0}},
            {{0,0,0,0},{0,0,1,1}},
            {{0,0,1,1},{0,0,0,0}},
            {{0,1,0,1},{0,1,0,1}},

            {{1,0,0,0},{1,0,0,0}},
            {{1,0,0,0},{1,0,1,1}},
            {{1,0,1,1},{1,0,0,0}},
            {{1,1,0,1},{1,1,0,1}},

        },

    }, //4

};


//#define GIG_TABLE
#ifdef  GIG_TABLE
static double ramp_err[LOG_CL_RANGE-LOG_CL_BASE][BIT_RANGE-BIT_BASE][256][256][256][16];
#endif

static double ramp[LOG_CL_RANGE-LOG_CL_BASE][BIT_RANGE-BIT_BASE][256][256][16];
static double ep_d[BIT_RANGE-BIT_BASE][256];
// inverted table
// <log2 clusters >,  bits, value, par1, par2, <ep1>
static int      sp_idx[LOG_CL_RANGE-LOG_CL_BASE][BIT_RANGE-BIT_BASE][256][2][2][MAX_CLUSTERS_BIG][2];
// <log2 clusters >,  bits, value, par1, par2,
static double   sp_err[LOG_CL_RANGE-LOG_CL_BASE][BIT_RANGE-BIT_BASE][256][2][2][MAX_CLUSTERS_BIG];
//#endif

//
int expand_ (int bits, int v) {
    assert(bits >=4);
    return (  v << (8-bits) | v >> (2* bits - 8));
}

static bool ramp_init = false;

void init_ramps (void) {
    if (ramp_init) return;
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    int clog1;
    int bits;
    int p1;
    int p2;
    int i,j;
    int o1,o2;

    for (bits=BIT_BASE; bits<BIT_RANGE; bits++)
        for (p1=0; p1<(1<<bits); p1++)
            ep_d[BTT(bits)][p1]=(double) expand_(bits,p1);


    for (clog1=LOG_CL_BASE; clog1<LOG_CL_RANGE; clog1++)
        for (bits=BIT_BASE; bits<BIT_RANGE; bits++)
            for (p1=0; p1<(1<<bits); p1++)
                for (p2=0; p2<(1<<bits); p2++) {
                    for (i=0; i<(1<<clog1); i++)
                        ramp[CLT(clog1)][BTT(bits)][p1][p2][i] =
                            floor(
                                (double) ep_d[BTT(bits)][p1] + rampLerpWeights[clog1][i] * (double)((ep_d[BTT(bits)][p2]- ep_d[BTT(bits)][p1]))
                                +0.5);

#ifdef GIG_TABLE
                    double v;
                    int vi;
                    for (vi=0; vi<256; vi++)
                        for (i=0; i<(1<<clog1); i++)
                            ramp_err[CLT(clog1)][BTT(bits)][p1][p2][vi][i] =
                                (ramp[CLT(clog1)][BTT(bits)][p1][p2][i]-(double) vi) *
                                (ramp[CLT(clog1)][BTT(bits)][p1][p2][i]-(double) vi);
#endif
                }


    //-----------------------------------------------------------------------------

    for (clog1=LOG_CL_BASE; clog1<LOG_CL_RANGE; clog1++)
        for (bits=BIT_BASE; bits<BIT_RANGE; bits++)
            for(j=0; j<256; j++)
                for (o1=0; o1<2; o1++)
                    for (o2=0; o2<2; o2++)
                        for(i=0; i<16; i++) {
                            sp_idx[CLT(clog1)][BTT(bits)][j][o1][o2][i][0]=-1;
                            sp_err[CLT(clog1)][BTT(bits)][j][o1][o2][i]=DBL_MAX;
                        }

    for (clog1=LOG_CL_BASE; clog1<LOG_CL_RANGE; clog1++)
        for (bits=BIT_BASE; bits<BIT_RANGE; bits++)
            for (p1=0; p1<(1<<bits); p1++)
                for (p2=0; p2<(1<<bits); p2++)
                    for (i=0; i<(1<<clog1); i++) {
                        sp_idx[CLT(clog1)][BTT(bits)][(int) ramp[CLT(clog1)][BTT(bits)][p1][p2][i]][p1 & 0x1][p2 & 0x1][i][0]=p1;
                        sp_idx[CLT(clog1)][BTT(bits)][(int) ramp[CLT(clog1)][BTT(bits)][p1][p2][i]][p1 & 0x1][p2 & 0x1][i][1]=p2;
                        sp_err[CLT(clog1)][BTT(bits)][(int) ramp[CLT(clog1)][BTT(bits)][p1][p2][i]][p1 & 0x1][p2 & 0x1][i]=0.;
                    }

    for (clog1=LOG_CL_BASE; clog1<LOG_CL_RANGE; clog1++)
        for (bits=BIT_BASE; bits<BIT_RANGE; bits++)
            for(j=0; j<256; j++)
                for (o1=0; o1<2; o1++)
                    for (o2=0; o2<2; o2++)
                        for(i=0; i<(1<<clog1); i++)
                            if (sp_idx[CLT(clog1)][BTT(bits)][j][o1][o2][i][0]<0) {
                                int k;
                                for (k=1; k<256; k++)
                                    if ( (j-k >= 0 && sp_err[CLT(clog1)][BTT(bits)][j-k][o1][o2][i]==0) ||
                                            (j+k < 256 && sp_err[CLT(clog1)][BTT(bits)][j+k][o1][o2][i]==0) )
                                        break;
                                {
                                    if ( (j-k >= 0 && sp_err[CLT(clog1)][BTT(bits)][j-k][o1][o2][i]==0)) {
                                        sp_idx[CLT(clog1)][BTT(bits)][j][o1][o2][i][0]=sp_idx[CLT(clog1)][BTT(bits)][j-k][o1][o2][i][0];
                                        sp_idx[CLT(clog1)][BTT(bits)][j][o1][o2][i][1]=sp_idx[CLT(clog1)][BTT(bits)][j-k][o1][o2][i][1];
                                    } else if ((j+k < 256 && sp_err[CLT(clog1)][BTT(bits)][j+k][o1][o2][i]==0)) {
                                        sp_idx[CLT(clog1)][BTT(bits)][j][o1][o2][i][0]=sp_idx[CLT(clog1)][BTT(bits)][j+k][o1][o2][i][0];
                                        sp_idx[CLT(clog1)][BTT(bits)][j][o1][o2][i][1]=sp_idx[CLT(clog1)][BTT(bits)][j+k][o1][o2][i][1];
                                    }
                                    sp_err[CLT(clog1)][BTT(bits)][j][o1][o2][i]=k*k;
                                }
                            }

    ramp_init = true;
}

// finds "floor in the set" if exists, otherwise returns min
inline int ep_find_floor( double v, int bits, int use_par, int odd) {
    assert(use_par==0 || use_par==1 || odd==0 || odd==1 );

    double *p = ep_d[BTT(bits)];
    int i1=0;
    int i2=1<<(bits-use_par);
    odd = use_par ? odd : 0;
    while (i2-i1>1) {
        int j = (i1+i2)/2;
        if (v >= p[(j<<use_par)+odd] )
            i1=j;
        else
            i2=j;
    }
    return (i1<<use_par)+odd;
}

// find closest one
inline int ep_find_near( double v, int bits, int use_par, int odd) {
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    assert(use_par==0 || use_par==1 || odd==0 || odd==1 ) ;
    double *p = ep_d[BTT(bits)];
    int p1 = ep_find_floor(v, bits, use_par,odd);
    int p2 = p1+(1<<use_par);
    p2 = p2 < (1<<bits) ? p2 : p1;
    if (fabs(v-p[p2]) < fabs(v-p[p1]))
        return p2;
    else
        return p1;
}

inline void mean_d (double d[][DIMENSION], double mean[DIMENSION], int n) {
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    int i,j;
    assert(n!=0);
    for(j=0; j< DIMENSION; j++)
        mean[j] =0;
    for(i=0; i< n; i++)
        for(j=0; j< DIMENSION; j++)
            mean[j] +=d[i][j];
    for(j=0; j< DIMENSION; j++)
        mean[j] /=(double) n;
}

inline void mean_d_d (double d[][MAX_DIMENSION_BIG], double mean[MAX_DIMENSION_BIG], int n, int dimension) {
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    int i,j;
    assert(n!=0);
    for(j=0; j< dimension; j++)
        mean[j] =0;
    for(i=0; i< n; i++)
        for(j=0; j< dimension; j++)
            mean[j] +=d[i][j];
    for(j=0; j< dimension; j++)
        mean[j] /=(double) n;
}

inline int cluster_mean_d (double d[][DIMENSION],  double mean[][DIMENSION], int index[],int i_comp[],int i_cnt[], int n) {
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    // unused index values are underfined
    int i,j,k;
    assert(n!=0);

    for(i=0; i< n; i++)
        for(j=0; j< DIMENSION; j++) {
            assert(index[i]<MAX_CLUSTERS_BIG);
            mean[index[i]][j] =0;
            i_cnt[index[i]]=0;
        }
    k=0;
    for(i=0; i< n; i++) {
        for(j=0; j< DIMENSION; j++)
            mean[index[i]][j] +=d[i][j];
        if (i_cnt[index[i]]==0)
            i_comp[k++]=index[i];
        i_cnt[index[i]]++;
    }

    for(i=0; i< k; i++)
        for(j=0; j< DIMENSION; j++)
            mean[i_comp[i]][j] /=(double) i_cnt[i_comp[i]];
    return k;
}

inline int cluster_mean_d_d (double d[][MAX_DIMENSION_BIG],  double mean[][MAX_DIMENSION_BIG], int index[],int i_comp[],int i_cnt[], int n, int dimension) {
    // unused index values are underfined
    int i,j,k;
    assert(n!=0);

    for(i=0; i< n; i++)
        for(j=0; j< dimension; j++) {
            assert(index[i]<MAX_CLUSTERS_BIG);
            mean[index[i]][j] =0;
            i_cnt[index[i]]=0;
        }
    k=0;
    for(i=0; i< n; i++) {
        for(j=0; j< dimension; j++)
            mean[index[i]][j] +=d[i][j];
        if (i_cnt[index[i]]==0)
            i_comp[k++]=index[i];
        i_cnt[index[i]]++;
    }

    for(i=0; i< k; i++)
        for(j=0; j< dimension; j++)
            mean[i_comp[i]][j] /=(double) i_cnt[i_comp[i]];
    return k;
}

inline int all_same (double d[][DIMENSION],  int n) {
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    assert(n>0);
    int i,j;
    int same = 1;
    for(i=1; i< n; i++)
        for(j=0; j< DIMENSION; j++)
            same = same && (d[0][j] ==d[i][j]);

    return(same);
}

inline int all_same_d (double d[][MAX_DIMENSION_BIG],  int n, int dimension) {
    assert(n>0);
    int i,j;
    int same = 1;
    for(i=1; i< n; i++)
        for(j=0; j< dimension; j++)
            same = same && (d[0][j] ==d[i][j]);

#ifdef USE_DBGTRACE
    DbgTrace(("[%d]",same));
#endif

    return(same);
}

inline int max_i (int a[], int n) {
    assert(n>0);
    int i,m=a[0];
    for(i=0; i< n; i++)
        m = m > a[i] ? m : a[i];
    return (m);
}

void index_collapse_ (int index[], int numEntries) {
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    int k;
    int d,D;
    int mi;
    int Mi;
    if (numEntries ==0)
        return;

    mi=Mi=index[0];
    for (k=1; k<numEntries; k++) {
        mi = mi < index[k] ? mi : index[k];
        Mi = Mi > index[k] ? Mi : index[k];
    }
    D=1;
    for (d=2; d<=Mi-mi; d++) {

        for (k=0; k<numEntries; k++)
            if ((index[k] -mi) % d !=0)
                break;
        if (k>=numEntries)
            D =d;
    }
    for (k=0; k<numEntries; k++)
        index[k] = (index[k]- mi)/D;
}

//
// New version that will take varying dimension
//
//
//

double BC7BlockEncoder::quant_single_point_d
(
    double data[MAX_ENTRIES][MAX_DIMENSION_BIG],
    int numEntries, int index[MAX_ENTRIES],
    double out[MAX_ENTRIES][MAX_DIMENSION_BIG],
    int epo_1[2][MAX_DIMENSION_BIG],
    int Mi_,                // last cluster
    int bits[3],            // including parity
    int type,
    int dimension
) {
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif

    int i,j;

    double err_0=DBL_MAX;

    double err_1=DBL_MAX;
    int idx = 0;
    int idx_1 = 0;

    int epo_0[2][MAX_DIMENSION_BIG];

    int use_par =(type !=0);

    int clog2=0;
    i = Mi_+1;
    while (i>>=1)
        clog2++;
    assert((1<<clog2)== Mi_+1);

    int pn;
    int (*pv)[MAX_DIMENSION_BIG];
    for (pn=0; pn<npv_nd[dimension][type]; pn++) {
        {
            pv = par_vectors_nd[dimension][type][pn];

            int o1[2][MAX_DIMENSION_BIG]= {0,2};
            int o2[2][MAX_DIMENSION_BIG]= {0,2};

            for (j=0; j<dimension; j++) {
                o2[0][j]=o1[0][j]=0;
                o2[1][j]=o1[1][j]=2;

                if (use_par) {
                    if (pv[0][j])
                        o1[0][j]=1;
                    else
                        o1[1][j]=1;
                    if (pv[1][j])
                        o2[0][j]=1;
                    else
                        o2[1][j]=1;
                }
            };

            int t1,t2;

            int dr[MAX_DIMENSION_BIG];
            int dr_0[MAX_DIMENSION_BIG];
            double tr;

            for (i=0; i< (1<<clog2); i++) {
                double t=0;
                int t1o[MAX_DIMENSION_BIG],t2o[MAX_DIMENSION_BIG];

                for (j=0; j<dimension; j++) {
                    double t_=DBL_MAX;

                    for (t1=o1[0][j]; t1<o1[1][j]; t1++) {
                        for (t2=o2[0][j]; t2<o2[1][j]; t2++)
                            // This is needed for non-integer mean points of "collapsed" sets
                        {
                            int tf = (int) floor(data[0][j]);
                            int tc = (int) ceil (data[0][j]);
                            // if data[o][j] is integer the above are equal, and so are errors

                            assert(tf >=0 && tc <=255.);

                            if (  sp_err[CLT(clog2)][BTT(bits[j])][tf][t1][t2][i]
                                    > sp_err[CLT(clog2)][BTT(bits[j])][tc][t1][t2][i])
                                // if they are not equal, the same representalbe point is used for
                                // both of them, as all representable points are integers in the rage
                                dr[j]=tc;
                            else if (sp_err[CLT(clog2)][BTT(bits[j])][ tf ][t1][t2][i]
                                     < sp_err[CLT(clog2)][BTT(bits[j])][ tc ][t1][t2][i])
                                dr[j]=tf;
                            else
                                dr[j]=(int)floor(data[0][j]+0.5);

                            tr = sp_err[CLT(clog2)][BTT(bits[j])][dr[j]][t1][t2][i] +
                                 2*sqrt(sp_err[CLT(clog2)][BTT(bits[j])][dr[j]][t1][t2][i]) * fabs((double)dr[j]-data[0][j])+
                                 (dr[j]-data[0][j])* (dr[j]-data[0][j]);

                            if (tr < t_) {
                                t_=tr;
                                t1o[j]=t1;
                                t2o[j]=t2;
                                dr_0[j]=dr[j];
                            }
                        }
                    }

                    t += t_;
                }

                if (t < err_0) {
                    idx=i;
                    for (j=0; j<dimension; j++) {
                        epo_0[0][j]=sp_idx[CLT(clog2)][BTT(bits[j])][ dr_0[j]][t1o[j]][t2o[j]][i][0];
                        epo_0[1][j]=sp_idx[CLT(clog2)][BTT(bits[j])][ dr_0[j]][t1o[j]][t2o[j]][i][1];
                    }
                    err_0=t;
                }
                if (err_0==0)
                    break;
            }

            if (err_0 < err_1) {
                idx_1=idx;
                for (j=0; j<dimension; j++) {
                    epo_1[0][j]=epo_0[0][j];
                    epo_1[1][j]=epo_0[1][j];
                }
                err_1=err_0;
            }

            if (err_1==0)
                break;
        }
    }

    for (i=0; i< numEntries; i++) {
        index[i]=idx_1;
        for (j=0; j<dimension; j++) {
            out[i][j]=ramp[CLT(clog2)][BTT(bits[j])][epo_1[0][j]][epo_1[1][j]][idx_1];
        }
    }
    return err_1 * numEntries;
}

double BC7BlockEncoder::ep_shaker_2_d(
    double data[MAX_ENTRIES][MAX_DIMENSION_BIG],
    int numEntries,
    int index_[MAX_ENTRIES],
    double out[MAX_ENTRIES][MAX_DIMENSION_BIG],
    int epo_code[2][MAX_DIMENSION_BIG],
    int size,
    int Mi_,             // last cluster
    int bits,            // total for all channels
    // defined by total numbe of bits and dimensioin
    int dimension,
    double epo[2][MAX_DIMENSION_BIG]

) {
#ifdef USE_DBGTRACE
    DbgTrace(("<-------------------->"));
#endif
    int i,j,k;

//
//###############################
// decode for "new style" interface, this can be passed in directly
//
    int max_bits[MAX_DIMENSION_BIG];
    int type = bits % (2*dimension);

    int use_par =(type !=0);
    for (j=0; j<dimension; j++)
        max_bits[j] = (bits+2*dimension-1) / (2*dimension);
//
//###############################
//

    int clog3=0;
    i = Mi_+1;
    while (i>>=1)
        clog3++;

    assert((1<<clog3)== Mi_+1);

    double mean[MAX_DIMENSION_BIG];
    int index[MAX_ENTRIES];
    int Mi;

    int maxTry=8;

    for (k=0; k<numEntries; k++)
        index[k]=index_[k];


    int done;
    int change;

    int better;

    double err_o = DBL_MAX;

    int epo_0[2][MAX_DIMENSION_BIG];

    double outg[MAX_ENTRIES][MAX_DIMENSION_BIG];

    // handled below automatically
    int alls= all_same_d(data, numEntries, dimension);
    mean_d_d (data, mean, numEntries, dimension);

    do {
        index_collapse_ (index, numEntries);

        Mi= max_i(index, numEntries);  // index can be from requantizer

        int p, q;
        int p0=-1,q0=-1;

        double err_0 = DBL_MAX;

#ifdef USE_DBGTRACE
        DbgTrace(("Mi [%2d] numEntries [%2d]",Mi,numEntries));
#endif

        if (Mi==0) {
            double t;
            // either single point from the beginning or collapsed index
            if (alls) {
                t =  quant_single_point_d( data,numEntries,index, outg, epo_0,  Mi_, max_bits,type, dimension);
            } else {
                quant_single_point_d(&mean,numEntries,index, outg, epo_0, Mi_, max_bits, type, dimension);
                t = totalError_d(data,outg,numEntries, dimension);
            }

            if (t < err_o) {
                for (k=0; k<numEntries; k++) {
                    index_[k]=index[k];
                    for (j=0; j<dimension; j++) {
                        out[k][j]=outg[k][j];
                        epo_code[0][j]=epo_0[0][j];
                        epo_code[1][j]=epo_0[1][j];
                    }
                };
                err_o=t;
            }

            for(j=0; j<dimension; j++) {
                epo[0][j]= ramp[CLT(clog3)][BTT(max_bits[j])][epo_code[0][j]][epo_code[1][j]][0];
                epo[1][j]= ramp[CLT(clog3)][BTT(max_bits[j])][epo_code[0][j]][epo_code[1][j]][(1<<clog3)-1];
            }

            return err_o;
        }

//          set stuff for  collapsed index shake (if needed)??
//          if it did collaps should we just check if it's better and get out ?

        assert(Mi <= Mi_);

        int cluster_mean_calls = 0;

        for (q=1; Mi!=0 && q*Mi <= Mi_; q++) // does not work for single point collapsed index!!!
            for (p=0; p<=Mi_-q*Mi; p++) {
                int cidx[MAX_ENTRIES];

                for (k=0; k<numEntries; k++)
                    cidx[k]=index[k] * q + p;

                double epa[2][MAX_DIMENSION_BIG];

                {
                    //
                    // solve RMS problem for center
                    //

                    double im [2][2] = {{0,0},{0,0}};   // matrix /inverse matrix
                    double rp[2][MAX_DIMENSION_BIG];            // right part for RMS fit problem

                    // get ideal clustr centers
                    double cc[MAX_CLUSTERS_BIG][MAX_DIMENSION_BIG];
                    int i_cnt[MAX_CLUSTERS_BIG]; // count of index entries
                    int i_comp[MAX_CLUSTERS_BIG];   // compacted index
                    int ncl;                        // number of unique indexes

                    cluster_mean_calls++; // used for debugging code: Remove when optimizing
                    ncl=cluster_mean_d_d (data,  cc, cidx, i_comp, i_cnt, numEntries, dimension); // unrounded

                    // round
                    for (i=0; i<ncl; i++)
                        for (j=0; j<dimension; j++)
                            cc[i_comp[i]][j]=floor(cc[i_comp[i]][j]+0.5); // more or less ideal location

                    for (j=0; j<dimension; j++)
                        rp[0][j]=rp[1][j]=0;

                    // weight with cnt if runnning on compacted index
                    for (k=0; k<numEntries; k++) {
                        im[0][0] += (Mi_-cidx[k])* (Mi_-cidx[k]);
                        im[0][1] +=      cidx[k] * (Mi_-cidx[k]);           // im is symmetric
                        im[1][1] +=      cidx[k] *      cidx[k];

                        for (j=0; j<dimension; j++) {
                            rp[0][j]+=(Mi_-cidx[k]) * cc[cidx[k]][j];
                            rp[1][j]+=     cidx[k]  * cc[cidx[k]][j];
                        }
                    }

                    double dd = im[0][0]*im[1][1]-im[0][1]*im[0][1];

                    assert(dd !=0);

                    // dd=0 means that cidx[k] and (Mi_-cidx[k]) collinear which implies only one active index;
                    // taken care of separately

                    im[1][0]=im[0][0];
                    im[0][0]=im[1][1]/dd;
                    im[1][1]=im[1][0]/dd;
                    im[1][0]=im[0][1]=-im[0][1]/dd;

                    for (j=0; j<dimension; j++) {
                        epa[0][j]=(im[0][0]*rp[0][j]+im[0][1]*rp[1][j])*Mi_;
                        epa[1][j]=(im[1][0]*rp[0][j]+im[1][1]*rp[1][j])*Mi_;
                    }
                }
                // shake single or                                   - cartesian
                // shake odd/odd and even/even or                    - same parity
                // shake odd/odd odd/even , even/odd and even/even   - bcc

                double err_1 = DBL_MAX;
                int epo_1[2][MAX_DIMENSION_BIG];

                double ed[2][2][MAX_DIMENSION_BIG];
                int epo_2_[2][2][2][MAX_DIMENSION_BIG];

                for (j=0; j<dimension; j++) {
                    double  (*rb) [256][16] =ramp[CLT(clog3)][BTT(max_bits[j])];

                    int pp[2]= {0,0};
                    int rr = (use_par ? 2:1);

                    int epi[2][2];  // first/second, coord, begin rage end range


                    for (pp[0]=0; pp[0]<rr; pp[0]++) {
                        for (pp[1]=0; pp[1]<rr; pp[1]++) {

                            for (i=0; i<2; i++) {   // set range
                                epi[i][0]= epi[i][1]= ep_find_floor( epa[i][j],max_bits[j], use_par, pp[i]);

                                epi[i][0]-= ( (epi[i][0]  < (size>>1)-1 ? epi[i][0] : (size>>1)-1 ) )  & (~use_par);

                                epi[i][1]+= ((1<<max_bits[j])-1 - epi[i][1]  < (size>>1) ?
                                             (1<<max_bits[j])-1 - epi[i][1]  : (size>>1) ) & (~use_par);
                            }
                            int p1,p2, step=(1<<use_par);

                            ed[pp[0]][pp[1]][j]=DBL_MAX;

                            for (p1=epi[0][0]; p1<=epi[0][1]; p1+=step)
                                for (p2=epi[1][0]; p2<=epi[1][1]; p2+=step) {
                                    double *rbp = rb[p1][p2];
                                    double t=0;
                                    int    *ci=cidx;
                                    int    m =numEntries;


                                    int _mc = m;

                                    while(_mc > 0) {
                                        t += (rbp[ci[_mc-1]]-data[_mc-1][j])*(rbp[ci[_mc-1]]-data[_mc-1][j]);
                                        _mc--;
                                    }

                                    if (t<ed[pp[0]][pp[1]][j]) {
                                        ed[pp[0]][pp[1]][j]=t;
                                        epo_2_[pp[0]][pp[1]][0][j]=p1;
                                        epo_2_[pp[0]][pp[1]][1][j]=p2;
                                    }
                                }
                        }
                    }
                }

                int pn;
                int (*pv)[MAX_DIMENSION_BIG];
                for (pn=0; pn<npv_nd[dimension][type]; pn++) {
                    pv = par_vectors_nd[dimension][type][pn];
                    int j1;
                    double err_2=0;
                    for (j1=0; j1<dimension; j1++)
                        err_2+=ed[pv[0][j1]][pv[1][j1]][j1];
                    if (err_2 < err_1) {
                        err_1 = err_2;
                        for (j1=0; j1<dimension; j1++) {
                            epo_1[0][j1]=epo_2_[pv[0][j1]][pv[1][j1]][0][j1];
                            epo_1[1][j1]=epo_2_[pv[0][j1]][pv[1][j1]][1][j1];
                        }
                    }
                }



                if (err_1 <= err_0) { // we'd want to get expanded index;
                    err_0 = err_1;
                    p0=p;
                    q0=q;
                    for (j=0; j<dimension; j++) {
                        epo_0[0][j]=epo_1[0][j];
                        epo_0[1][j]=epo_1[1][j];
                    }
                }
            }


#ifdef USE_DBGTRACE
        DbgTrace(("cluster_mean_d_d [%2d] q[%d] p[%d]",cluster_mean_calls,q,p));
#endif

        // requantize
        double *r[MAX_DIMENSION_BIG];
        int idg[MAX_ENTRIES];

        double err_r=0;

        for (j=0; j<dimension; j++)
            r[j]= ramp[CLT(clog3)][BTT(max_bits[j])][epo_0[0][j]][epo_0[1][j]];

        for (i=0; i<numEntries; i++) {
            double  cmin = DBL_MAX;
            int        ci = 0;
            double    *d=data[i];

            for(j=0; j < (1<<clog3); j++) {
                double t_=0.;

                for(k=0; k<dimension; k++) {
                    t_+=(r[k][j]-d[k])*(r[k][j]-d[k]);
                }

                if(t_<cmin) {
                    cmin = t_;
                    ci = j;
                }
            }

            idg[i]=ci;
            for(k=0; k<dimension; k++) {
                outg[i][k]=r[k][ci];
            }

            err_r+=cmin;
        }

        // change/better
        change =0;
        for (k=0; k<numEntries; k++)
            change = change || (index[k] * q0 + p0!=idg[k]);

        better = err_r < err_o;

        if (better) {
            for (k=0; k<numEntries; k++) {
                index_[k]=index[k]=idg[k];
                for (j=0; j<dimension; j++) {
                    out[k][j]=outg[k][j];
                    epo_code[0][j]=epo_0[0][j];
                    epo_code[1][j]=epo_0[1][j];
                }
            }
            err_o=err_r;
        }

        done = !(change  &&  better) ;

    } while (! done && maxTry--);

    for(j=0; j<dimension; j++) {
        epo[0][j]= ramp[CLT(clog3)][BTT(max_bits[j])][epo_code[0][j]][epo_code[1][j]][0];
        epo[1][j]= ramp[CLT(clog3)][BTT(max_bits[j])][epo_code[0][j]][epo_code[1][j]][(1<<clog3)-1];
    }

    return err_o;
}




double BC7BlockEncoder::ep_shaker_d(
    double data[MAX_ENTRIES][MAX_DIMENSION_BIG],
    int numEntries,
    int index_[MAX_ENTRIES],
    double out[MAX_ENTRIES][MAX_DIMENSION_BIG],
    int epo_code[2][MAX_DIMENSION_BIG],
    int Mi_,                // last cluster
    int bits[3],            // including parity
    CMP_qt_cpu type,
    int dimension
) {
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    //###############################
    int i,j,k;

    int use_par =(type == BCC || type == SAME_PAR);
    int bcc = (type == BCC);
    int clog4=0;
    i = Mi_+1;
    while (i>>=1)
        clog4++;

    assert((1<<clog4)== Mi_+1);

    //###########################

    double mean[MAX_DIMENSION_BIG];
    int index[MAX_ENTRIES];
    int Mi;

    int maxTry=1;

    for (k=0; k<numEntries; k++)
        index[k]=index_[k];

    int done;
    int change;

    int better;

    double err_o = DBL_MAX;

    // handled below automatically
    int alls= all_same_d(data, numEntries, dimension);

    mean_d_d(data, mean, numEntries, dimension);

    do {
        index_collapse_ (index, numEntries);

        Mi= max_i(index, numEntries);  // index can be from requantizer

        int p, q;
        int p0=-1,q0=-1;

        double err_2 = DBL_MAX;
        double out_2[MAX_ENTRIES][MAX_DIMENSION_BIG];
        int idx_2[MAX_ENTRIES];
        int    epo_2[2][MAX_DIMENSION_BIG];

        if (Mi==0) {
            double t;
            int    epo_0[2][MAX_DIMENSION_BIG];
            // either sinle point from the beginning or collapsed index
            if (alls) {
                t =  quant_single_point_d( data,numEntries,index, out_2, epo_0, Mi_, bits,  type, dimension);
            } else {
                quant_single_point_d(&mean,numEntries,index, out_2, epo_0, Mi_, bits,  type, dimension);
                t = totalError_d(data,out_2,numEntries, dimension);
            }

            if (t < err_o) {
                for (k=0; k<numEntries; k++) {
                    index_[k]=index[k];
                    for (j=0; j<dimension; j++) {
                        out[k][j]=out_2[k][j];
                        epo_code[0][j]=epo_0[0][j];
                        epo_code[1][j]=epo_0[1][j];
                    }
                };
                err_o=t;
            }
            return err_o;
        }


//          set stuff for  collapsed index shake (if needed)??
//            if it did collaps should we just check if it's better and get out ?

        int cluster_mean_calls = 0;

        for (q=1; Mi!=0 && q*Mi <= Mi_; q++) { // does not work for single point collapsed index!!!
            for (p=0; p<=Mi_-q*Mi; p++) {
                int cidx[MAX_ENTRIES];

                for (k=0; k<numEntries; k++) {
                    cidx[k]=index[k] * q + p;
                }

                double epa[2][MAX_DIMENSION_BIG];

                {
                    //
                    // solve RMS problem for center
                    //

                    double im [2][2] = {{0,0},{0,0}};   // matrix /inverse matrix
                    double rp[2][MAX_DIMENSION_BIG];            // right part for RMS fit problem

                    // get ideal clustr centers
                    double cc[MAX_CLUSTERS_BIG][MAX_DIMENSION_BIG];
                    int i_cnt[MAX_CLUSTERS_BIG]; // count of index entries
                    int i_comp[MAX_CLUSTERS_BIG];   // compacted index
                    int ncl;                        // number of unique indexes

                    cluster_mean_calls++; // used for debugging code: Remove when optimizing
                    ncl=cluster_mean_d_d (data,  cc, cidx, i_comp, i_cnt, numEntries, dimension); // unrounded

                    // round
                    for (i=0; i<ncl; i++)
                        for (j=0; j<dimension; j++)
                            cc[i_comp[i]][j]=floor(cc[i_comp[i]][j]+0.5); // more or less ideal location

                    for (j=0; j<dimension; j++) {
                        rp[0][j]=rp[1][j]=0;
                    }

                    // weight with cnt if runnning on compacted index
                    for (k=0; k<numEntries; k++) {
                        im[0][0] += (Mi_-cidx[k])* (Mi_-cidx[k]);
                        im[0][1] +=      cidx[k] * (Mi_-cidx[k]);           // im is symmetric
                        im[1][1] +=      cidx[k] *      cidx[k];

                        for (j=0; j<dimension; j++) {
                            rp[0][j]+=(Mi_-cidx[k]) * cc[cidx[k]][j];
                            rp[1][j]+=     cidx[k]  * cc[cidx[k]][j];
                        }
                    }

                    double dd = im[0][0]*im[1][1]-im[0][1]*im[0][1];

                    assert(dd !=0);

                    // dd=0 means that cidx[k] and (Mi_-cidx[k]) collinear which implies only one active index;
                    // taken care of separately

                    im[1][0]=im[0][0];
                    im[0][0]=im[1][1]/dd;
                    im[1][1]=im[1][0]/dd;
                    im[1][0]=im[0][1]=-im[0][1]/dd;

                    for (j=0; j<dimension; j++) {
                        epa[0][j]=(im[0][0]*rp[0][j]+im[0][1]*rp[1][j])*Mi_;
                        epa[1][j]=(im[1][0]*rp[0][j]+im[1][1]*rp[1][j])*Mi_;
                    }
                }
                // shake single or                                   - cartesian
                // shake odd/odd and even/even or                    - same parity
                // shake odd/odd odd/even , even/odd and even/even   - bcc
                int odd,flip1;

                double err_1 = DBL_MAX;
                double out_1[MAX_ENTRIES][MAX_DIMENSION_BIG];
                int idx_1[MAX_ENTRIES];
                int epo_1[2][MAX_DIMENSION_BIG];
                int s1 = 0;

                for (odd=0; odd<=use_par; odd++) {
                    for (flip1=0; flip1 <= bcc; flip1++) {
                        // flip partiy on the second point
                        int epi[2][MAX_DIMENSION_BIG][2];   // first second, coord, begin rage end range

                        for (j=0; j<dimension; j++) {
                            for (i=0; i<2; i++) {
                                // set range
                                epi[i][j][0]= epi[i][j][1]= ep_find_floor( epa[i][j], bits[j], use_par, (odd ^ (flip1 & i)) & 0x1);

                                epi[i][j][1]+= ((1<<bits[j])-1 - epi[i][j][1]  < (1<<use_par) ?
                                                (1<<bits[j])-1 - epi[i][j][1]  : (1<<use_par) ) & (~use_par);
                            }
                        }

                        double *r[MAX_DIMENSION_BIG];

                        double ce[MAX_ENTRIES][MAX_CLUSTERS_BIG][MAX_DIMENSION_BIG];

                        for (j=0; j<dimension; j++)
                            r[j]= ramp[CLT(clog4)][BTT(bits[j])][epi[0][j][0]][epi[1][j][0]];

                        double err_0 = 0;
                        double out_0[MAX_ENTRIES][MAX_DIMENSION_BIG];
                        int idx_0[MAX_ENTRIES];


                        for(i=0; i<numEntries; i++) {
                            double *d=data[i];
                            for(j=0; j<(1<<clog4); j++)
                                for(k=0; k<dimension; k++)
                                    ce[i][j][k] = (r[k][j]-d[k])*(r[k][j]-d[k]);
                        }

                        int s=0, p1, g;
                        int ei0=0, ei1=0;

                        for (p1 =0; p1<64 ; p1++) {
                            int j0=0;

                            // Gray code increment
                            g = p1 & (-p1);

                            err_0=0;

                            for (j=0; j<dimension; j++) {
                                if ( ((g >> (2 *j)) & 0x3) !=0) {
                                    j0 =j;
                                    // new cords
                                    ei0 = ( ( (s^g) >>(2 *j))    & 0x1);
                                    ei1 = ( ( (s^g) >>(2 *j+1))    & 0x1);
                                }
                            }
                            s = s ^ g;
                            r[j0]= ramp[CLT(clog4)][BTT(bits[j0])][epi[0][j0][ei0]][epi[1][j0][ei1]];

                            err_0 = 0;

                            for (i=0; i<numEntries; i++) {
                                double *d=data[i];
                                int    ci = 0;
                                double cmin = DBL_MAX;

                                for(j=0; j<(1<<clog4); j++) {
                                    double t_ = 0.;
                                    ce[i][j][j0] = (r[j0][j]-d[j0])*(r[j0][j]-d[j0]);

                                    for(k=0; k<dimension; k++) {
                                        t_ += ce[i][j][k];
                                    }

                                    if(t_< cmin) {
                                        cmin = t_;
                                        ci = j;
                                    }
                                }

                                idx_0[i]=ci;
                                for(k=0; k<dimension; k++) {
                                    out_0[i][k]=r[k][ci];
                                }
                                err_0+=cmin;
                            }

                            if (err_0 < err_1) {
                                // best in the curent ep cube run
                                for (i=0; i < numEntries; i++) {
                                    idx_1[i]=idx_0[i];
                                    for (j=0; j<dimension; j++)
                                        out_1[i][j]=out_0[i][j];
                                }
                                err_1=err_0;

                                s1=s; // epo coding
                            }
                        }

                        // reconstruct epo
                        for (j=0; j<dimension; j++) {
                            {
                                // new cords
                                ei0 = ( ( s1 >>(2 *j)  )    & 0x1);
                                ei1 = ( ( s1 >>(2 *j+1))    & 0x1);
                                epo_1[0][j]=epi[0][j][ei0];
                                epo_1[1][j]=epi[1][j][ei1];
                            }
                        }
                    }
                }
                if (err_1 < err_2) {
                    // best in the curent ep cube run
                    for (i=0; i < numEntries; i++) {
                        idx_2[i]=idx_1[i];
                        for (j=0; j<dimension; j++)
                            out_2[i][j]=out_1[i][j];
                    }
                    err_2=err_1;
                    for (j=0; j<dimension; j++) {
                        epo_2[0][j]=epo_1[0][j];
                        epo_2[1][j]=epo_1[1][j];
                    }
                    p0=p;
                    q0=q;
                }
            }
        }

#ifdef USE_DBGTRACE
        DbgTrace(("cluster_mean_d_d [%2d]",cluster_mean_calls));
#endif


        // change/better
        change =0;
        for (k=0; k<numEntries; k++)
            change = change || (index[k] * q0 + p0!=idx_2[k]);

        better = err_2 < err_o;

        if (better) {
            for (k=0; k<numEntries; k++) {
                index_[k]=index[k]=idx_2[k];
                for (j=0; j<dimension; j++) {
                    out[k][j]=out_2[k][j];
                    epo_code[0][j]=epo_2[0][j];
                    epo_code[1][j]=epo_2[1][j];
                }
            }
            err_o=err_2;
        }

        done = !(change  &&  better) ;

    } while (! done && maxTry--);

    return err_o;
}