//===============================================================================
// Copyright (c) 2007-2024  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 <math.h>
#include <string.h>
#include <assert.h>
#include "3dquant_constants.h"
#include "3dquant_vpc.h"
#include "reconstruct.h"
#include "debug.h"

static double maxStep[MAX_CLUSTERS]  = {0, 0, 0, 0, 0, 0, 0, 0};
static double maxStep1[MAX_CLUSTERS] = {0, 0, 0, 0, 0, 0, 0, 0};
static double minStep[MAX_CLUSTERS]  = {1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000};
static double minStep1[MAX_CLUSTERS] = {1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000};

static int stepHisto[512 * 16];
static int stepHisto1[512 * 16];
static int stepHistoI[MAX_SUBSETS][512 * 16];

static int histoInit = 0;

static double NShakeCnt = 0;
static double PCnt      = 0;

void histoStep(double step, double step1)
{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    if (histoInit == 0)
    {
        int i;
        for (i = 0; i < 512 * 16; i++)
            stepHisto[i] = stepHisto1[i] = 0;
        histoInit = 1;
    }
    step *= 16;
    step = step < 0 ? 0 : step;
    step = step > 512. * 16 ? 512. * 16 : step;
    stepHisto[(int)floor(step + 1 / 2.)]++;

    step1 *= 16;
    step1 = step1 < 0 ? 0 : step1;
    step1 = step1 > 512. * 16 ? 512. * 16 : step1;
    stepHisto1[(int)floor(step1 + 1 / 2.)]++;
};

void histoStepCnt(int id, int cnt, double step1)
{
    if (histoInit == 0)
    {
        int i;
        for (i = 0; i < 512 * 16; i++)
            stepHisto[i] = stepHisto1[i] = 0;
        histoInit = 1;
    }

    step1 *= 16;
    step1 = step1 < 0 ? 0 : step1;
    step1 = step1 > 512. * 16 ? 512. * 16 : step1;
    stepHistoI[id][(int)floor(step1 + 1 / 2.)] += cnt;
};

void printStepHistoI(void)
{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    int i, j, k, l;

    for (l = 0; l < MAX_SUBSETS; l++)
    {
        k = 0;
        printf("[\n");
        for (i = 0; i < 512 * 16 / 32; i++)
        {
            for (j = 0; j < 32; j++)
                printf("%d, ", stepHistoI[l][k++]);
            printf("\n");
        }

        printf("]\n");
    }
}

void printStepHisto(void)
{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    int i, j, k;
    k = 0;
    printf("[\n");
    for (i = 0; i < 512 * 16 / 32; i++)
    {
        for (j = 0; j < 32; j++)
            printf("%d, ", stepHisto[k++]);
        printf("\n");
    }

    printf("]\n");

    k = 0;
    printf("[\n");
    for (i = 0; i < 512 * 16 / 32; i++)
    {
        for (j = 0; j < 32; j++)
            printf("%d, ", stepHisto1[k++]);
        printf("\n");
    }

    printf("]\n");
}

void printStep(void)
{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    int i;
    for (i = 0; i < MAX_CLUSTERS; i++)
        printf("step %2d %10g %10g  %10g %10g \n", i, minStep[i], minStep1[i], maxStep[i], maxStep1[i]);
}
void printCnt(void)
{
    printf("total pix %g rounded %g percentage %g\n", PCnt, PCnt - NShakeCnt, (PCnt - NShakeCnt) / PCnt);
}

void index_collapse  // assymtric of x->n-x, but this does not matter below
    (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 = 0; k < numEntries; k++)
    {
        mi = mi < index[k] ? mi : index[k];
        Mi = Mi > index[k] ? Mi : index[k];
    }

    D = 1;

    for (d = 2; d <= 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;
}

void index_expand  // assymtric of x->n-x, but this does not matter below
    (int index[], int numEntries, int max_clusters)
{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    int k;
    int d;

    int Mi = 0, mi = max_clusters - 1;

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

    d = Mi - mi == 0 ? 1 : (max_clusters - 1) / (Mi - mi);

    for (k = 0; k < numEntries; k++)
        index[k] = (index[k] - mi) * d;
}

void sHisto(int index[], int numEntries, double step, double step1)
{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    int Mi = 0;
    int k;
    for (k = 0; k < numEntries; k++)
        Mi = Mi > index[k] ? Mi : index[k];

    if (step != 0.)
        minStep[Mi] = minStep[Mi] < step ? minStep[Mi] : step;

    maxStep[Mi] = maxStep[Mi] > step ? maxStep[Mi] : step;

    if (step1 != 0.)
        minStep1[Mi] = minStep1[Mi] < step1 ? minStep1[Mi] : step1;

    if (step1 != 0.)
        maxStep1[Mi] = maxStep1[Mi] > step1 ? maxStep1[Mi] : step1;
}

double reconstruct(double  data[MAX_ENTRIES][DIMENSION],
                   int     numEntries,
                   int     index_[MAX_ENTRIES],
                   double  out[MAX_ENTRIES][DIMENSION],
                   int     ns,
                   double  direction[DIMENSION],
                   double* step)
{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    double s = 0, t = 0, q = 0;
    int    i, j, k;
    double mean[DIMENSION];
    int    index[MAX_ENTRIES];

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

    // expand/collapse if needed

    if (ns == 1)  // 3 bit index
        index_expand(index, numEntries, 16);
    else
        index_expand(index, numEntries, MAX_CLUSTERS);

    for (j = 0; j < DIMENSION; j++)
    {
        for (mean[j] = k = 0; k < numEntries; k++)
            mean[j] += data[k][j];
        mean[j] /= (double)numEntries;
    }

    for (k = 0; k < numEntries; k++)
    {
        s += index[k];
        t += index[k] * index[k];
    }

    double q1 = 0;

    for (j = 0; j < DIMENSION; j++)
    {
        direction[j] = 0;
        for (k = 0; k < numEntries; k++)
            direction[j] += (data[k][j] - mean[j]) * index[k];

        q += direction[j] * direction[j];
        q1 = fabs(direction[j]) > q1 ? fabs(direction[j]) : q1;
    }

    s /= (double)numEntries;

    t = t - s * s * (double)numEntries;

    t = (t == 0 ? 0. : 1 / t);

    q = sqrt(q);

    *step = t * q;

    for (j = 0; j < DIMENSION; j++)
    {
        direction[j] /= (q1 == 0 ? 1 : q1);
        if (direction[j] >= 1.0001)
            printf("error\n");
    }

    if (*step != 0)
        *step *= q1 / q;

    for (i = 0; i < numEntries; i++)
        for (j = 0; j < DIMENSION; j++)
            out[i][j] = mean[j] + direction[j] * (*step) * (index[i] - s);

    // normalize direction for output

    return totalError(data, out, numEntries);
}

double reconstruct_new(double  data[MAX_ENTRIES][DIMENSION],
                       int     numEntries,
                       int     index[MAX_ENTRIES],
                       double  out[MAX_ENTRIES][DIMENSION],
                       int     ns,
                       double  direction[DIMENSION],
                       double* step)
{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    double s = 0, t = 0, q = 0;
    int    i, j, k;
    double mean[DIMENSION];

    // expand/collapse if needed

    if (ns == 1)  // 3 bit indes
        index_expand(index, numEntries, 16);
    else
        index_expand(index, numEntries, MAX_CLUSTERS);

    for (j = 0; j < DIMENSION; j++)
    {
        for (mean[j] = k = 0; k < numEntries; k++)
            mean[j] += data[k][j];
        mean[j] /= (double)numEntries;
    }

    for (k = 0; k < numEntries; k++)
    {
        s += index[k];
        t += index[k] * index[k];
    }

    double q1 = 0;

    for (j = 0; j < DIMENSION; j++)
    {
        direction[j] = 0;
        for (k = 0; k < numEntries; k++)
            direction[j] += (data[k][j] - mean[j]) * index[k];

        q += direction[j] * direction[j];
        q1 = fabs(direction[j]) > q1 ? fabs(direction[j]) : q1;
    }

    s /= (double)numEntries;

    t = t - s * s * (double)numEntries;

    t = (t == 0 ? 0. : 1 / t);

    q = sqrt(q);

    *step = t * q;

    for (j = 0; j < DIMENSION; j++)
    {
        direction[j] /= (q1 == 0 ? 1 : q1);
        if (direction[j] >= 1.0001)
            printf("error\n");
    }

    double step_l2 = *step;

    if (*step != 0)
        *step *= q1 / q;

    double step_li = *step;

    sHisto(index, numEntries, step_l2, step_li);
    histoStep(step_l2, step_li);

    for (i = 0; i < numEntries; i++)
        for (j = 0; j < DIMENSION; j++)
            out[i][j] = mean[j] + direction[j] * (*step) * (index[i] - s);

    return totalError(data, out, numEntries);
}

void shake(double data[MAX_ENTRIES][DIMENSION],
           int    numEntries,
           double idx_mean,

           double dir_r[4][DIMENSION],
           double mean_r[8][DIMENSION],
           double step_r[2],

           int    index_r[MAX_ENTRIES],
           double out[MAX_ENTRIES][DIMENSION])

{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    double c[MAX_CLUSTERS][DIMENSION];

    double t2[MAX_CLUSTERS];

    int index_t[MAX_ENTRIES];

    double s = idx_mean;

    double me = 512 * 512 * 3. * 16.;

    int p, q, r;
    int i, j, k;

    for (p = 0; p < 4; p++)
        for (q = 0; q < 8; q++)
            for (r = 0; r < 2; r++)
            {
                // generate sequence
                for (i = 0; i < MAX_CLUSTERS; i++)
                {
                    t2[i] = 0;
                    for (j = 0; j < DIMENSION; j++)
                    {
                        c[i][j] = floor(mean_r[q][j] + dir_r[p][j] * step_r[r] * (i - s) + 1 / 2.);

                        t2[i] += c[i][j] * c[i][j];
                    }
                }
                double ea = 0;

                for (i = 0; i < numEntries; i++)
                {
                    double e   = t2[0] + data[i][0] * c[0][0] + data[i][1] * c[0][1] + data[i][2] * c[0][2];
                    index_t[i] = 0;

                    for (k = 1; k < MAX_CLUSTERS; k++)
                    {
                        double t = t2[0] + data[i][0] * c[k][0] + data[i][1] * c[k][1] + data[i][2] * c[k][2];

                        if (t < e)
                        {
                            index_t[i] = i;
                            e          = t;
                        }
                    }
                    ea += e;
                }
                if (ea < me)
                {
                    me = ea;
                    for (i = 0; i < numEntries; i++)
                        index_r[i] = index_t[i];
                }
            }
    for (i = 0; i < numEntries; i++)
        for (j = 0; j < DIMENSION; j++)
            out[i][j] = c[index_r[i]][j];
}

void shake_d_s(double data[MAX_ENTRIES][DIMENSION],
               int    numEntries,
               double idx_mean,

               double dir_step_r[2][4][DIMENSION],
               double mean_r[8][DIMENSION],

               int    index_r[MAX_ENTRIES],
               double out[MAX_ENTRIES][DIMENSION])

{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
#define MAX_CLUSTERS_1_16 16

    double c[MAX_CLUSTERS_1_16][DIMENSION];

    double t2[MAX_CLUSTERS_1_16];

    int index_t[MAX_ENTRIES];

    double s = idx_mean;

    double me = 512 * 512 * 3. * 16.;
    double ea = 0;
    int    p, q, r;
    int    i, j, k;

    for (p = 0; p < 4; p++)
        for (q = 0; q < 8; q++)
            for (r = 0; r < 2; r++)
            {
                // generate sequence
                for (i = 0; i < MAX_CLUSTERS_1_16; i++)
                {
                    t2[i] = 0;
                    for (j = 0; j < DIMENSION; j++)
                    {
                        c[i][j] = floor(mean_r[q][j] + dir_step_r[r][p][j] * (i - s) + 1. / 2.);
                        c[i][j] = c[i][j] > 0 ? c[i][j] : 0;
                        c[i][j] = c[i][j] < 255. ? c[i][j] : 255.;

                        t2[i] += c[i][j] * c[i][j] / 2;  // negative
                    }
                }

                ea = 0;

                for (i = 0; i < numEntries; i++)
                {
                    double e   = t2[0] - data[i][0] * c[0][0] - data[i][1] * c[0][1] - data[i][2] * c[0][2];
                    index_t[i] = 0;

                    for (k = 1; k < MAX_CLUSTERS_1_16; k++)
                    {
                        double t = t2[k] - data[i][0] * c[k][0] - data[i][1] * c[k][1] - data[i][2] * c[k][2];

                        if (t < e)
                        {
                            index_t[i] = k;
                            e          = t;
                        }
                    }
                    ea += e;
                }
                if (ea < me)
                {
                    me = ea;
                    int i1, j1;

                    for (i1 = 0; i1 < numEntries; i1++)
                    {
                        index_r[i1] = index_t[i1];
                        for (j1 = 0; j1 < DIMENSION; j1++)
                            out[i1][j1] = c[index_r[i1]][j1];
                    }
                }
            }
}

void shake_d_s_s(double data[MAX_ENTRIES][DIMENSION],
                 int    numEntries,
                 int    numClusters,
                 double idx_mean,

                 double dir_step_r[2][4][DIMENSION],
                 double mean_step_r[2][8][DIMENSION],

                 int    index_r[MAX_ENTRIES],
                 double out[MAX_ENTRIES][DIMENSION])

{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    double c[MAX_CLUSTERS][DIMENSION];

    double t2[MAX_CLUSTERS];

    int index_t[MAX_ENTRIES];

    double s = idx_mean;

    double me = 512 * 512 * 3. * 16.;
    double ea = 0;
    int    p, q, r;
    int    i, j, k;

    for (p = 0; p < 4; p++)
        for (q = 0; q < 8; q++)
            for (r = 0; r < 2; r++)
            {
                // generate sequence
                for (i = 0; i < numClusters; i++)
                {
                    t2[i] = 0;
                    for (j = 0; j < DIMENSION; j++)
                    {
                        c[i][j] = floor(mean_step_r[r][q][j] + dir_step_r[r][p][j] * (i - s) + 1. / 2.);
                        c[i][j] = c[i][j] > 0 ? c[i][j] : 0;
                        c[i][j] = c[i][j] < 255. ? c[i][j] : 255.;

                        t2[i] += c[i][j] * c[i][j] / 2;  // negative
                    }
                }
                ea = 0;

                for (i = 0; i < numEntries; i++)
                {
                    double e   = t2[0] - data[i][0] * c[0][0] - data[i][1] * c[0][1] - data[i][2] * c[0][2];
                    index_t[i] = 0;

                    for (k = 1; k < numClusters; k++)
                    {
                        double t = t2[k] - data[i][0] * c[k][0] - data[i][1] * c[k][1] - data[i][2] * c[k][2];

                        if (t < e)
                        {
                            index_t[i] = k;
                            e          = t;
                        }
                    }
                    ea += e;
                }
                if (ea < me)
                {
                    me = ea;
                    int i1, j1;

                    for (i1 = 0; i1 < numEntries; i1++)
                    {
                        index_r[i1] = index_t[i1];
                        for (j1 = 0; j1 < DIMENSION; j1++)
                            out[i1][j1] = c[index_r[i1]][j1];
                    }
                }
            }
}

void mds(double  data[MAX_ENTRIES][DIMENSION],
         int     numEntries,
         int     index[MAX_ENTRIES],
         double  mean[DIMENSION],
         double* idx_mean,
         double  direction[DIMENSION],
         double* step)
{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    int    j, k;
    double t = 0, s = 0, q1 = 0;

    for (j = 0; j < DIMENSION; j++)
    {
        for (mean[j] = k = 0; k < numEntries; k++)
            mean[j] += data[k][j];
        mean[j] /= (double)numEntries;
    }

    for (k = 0; k < numEntries; k++)
    {
        s += index[k];
        t += index[k] * index[k];
    }

    for (j = 0; j < DIMENSION; j++)
    {
        direction[j] = 0;
        for (k = 0; k < numEntries; k++)
            direction[j] += (data[k][j] - mean[j]) * index[k];

        q1 = fabs(direction[j]) > q1 ? fabs(direction[j]) : q1;
    }

    *idx_mean = (s /= (double)numEntries);
    t         = t - s * s * (double)numEntries;
    t         = (t == 0 ? 0. : 1 / t);

    *step = t * q1;

    for (j = 0; j < DIMENSION; j++)
    {
        direction[j] /= (q1 == 0 ? 1 : q1);
        if (direction[j] >= 1.0001)
            printf("error\n");
    }
}

void mds_d(double  data[MAX_ENTRIES][DIMENSION],
           int     numEntries,
           int     index[MAX_ENTRIES],
           double  mean[DIMENSION],
           double* idx_mean,
           double  direction[DIMENSION],
           double* step)
{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    int    i, j, k;
    double t = 0, s = 0, q1 = 0;
    double cc[MAX_CLUSTERS_BIG][DIMENSION];
    int    cnt[MAX_CLUSTERS_BIG];

    for (i = 0; i < MAX_CLUSTERS_BIG; i++)
    {
        cnt[i] = 0;
        for (j = 0; j < DIMENSION; j++)
            cc[i][j] = 0;
    }

    for (k = 0; k < numEntries; k++)
    {
        cnt[index[k]]++;
        s += index[k];
        t += index[k] * index[k];
        for (j = 0; j < DIMENSION; j++)
            cc[index[k]][j] += data[k][j];
    }

    for (i = 0; i < MAX_CLUSTERS_BIG; i++)
        if (cnt[i])
            for (j = 0; j < DIMENSION; j++)
            {
                cc[i][j] /= (double)cnt[i];
                cc[i][j] = floor(cc[i][j] + 0.5);  // target discrete color
            }

    for (j = 0; j < DIMENSION; j++)
    {
        for (mean[j] = i = 0; i < MAX_CLUSTERS_BIG; i++)
            mean[j] += cc[i][j] * (double)cnt[i];
        mean[j] /= (double)numEntries;
    }

    for (j = 0; j < DIMENSION; j++)
    {
        direction[j] = 0;
        for (i = 0; i < MAX_CLUSTERS_BIG; i++)
            direction[j] += (cc[i][j] - mean[j]) * (double)cnt[i] * (double)i;

        q1 = fabs(direction[j]) > q1 ? fabs(direction[j]) : q1;
    }

    *idx_mean = (s /= (double)numEntries);
    t         = t - s * s * (double)numEntries;
    t         = (t == 0 ? 0. : 1 / t);

    *step = t * q1;

    for (j = 0; j < DIMENSION; j++)
    {
        direction[j] /= (q1 == 0 ? 1 : q1);
        if (direction[j] >= 1.0001)
            printf("error\n");
    }
}

//mean rounding setup
// cube model
void setMean(double mean[DIMENSION], double mr[8][DIMENSION], int div)
{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    double idiv = 1.;  // full scale divider per component
    int    sdiv = 1;   // Z/fcc/bcc swithc

    int j, k;

    while (div >= 8)
    {
        div /= 8;
        idiv *= 2.;
    }
    sdiv = div;

    // bcc*2 and cartesian; starting points for fcc
    mr[4][0] = (mr[0][0] = floor(mean[0] / idiv)) + 1.;
    if (sdiv == 4)
    {  // switch to search in bcc*2
        for (k = 0; k < 5; k += 4)
        {
            if ((int)floor(mr[k][0] + 0.5) & 1)
            {
                mr[k][1] = floor((mean[1] / idiv - 1.) / 2.) + 1. / 2.;
                mr[k][2] = floor((mean[2] / idiv - 1.) / 2.) + 1. / 2.;
            }
            else
            {
                mr[k][1] = floor(mean[1] / idiv / 2.);
                mr[k][2] = floor(mean[2] / idiv / 2.);
            }
        }
    }
    else
    {
        mr[4][1] = mr[0][1] = floor(mean[1] / idiv);
        mr[4][2] = mr[0][2] = floor(mean[2] / idiv);
    }

    // search in fcc
    for (k = 0; k < 5; k += 4)
    {
        if (sdiv == 2)
        {
            if ((int)floor(mr[k][0] + mr[k][1] + mr[k][2] + 0.5) & 1)
            {
                //latice point, leave [0] be,
                if (-mr[k][1] + mr[k][2] < -(mean[1] / idiv) + (mean[2] / idiv))
                {
                    // [-1,1]
                    mr[k][1]--;
                    mr[k][2]++;
                }  // else Ok to go
            }
            else
            {
                if (mr[k][1] + 1 + mr[k][2] < (mean[1] / idiv) + (mean[2] / idiv))
                    // get to [0,1] and go right
                    mr[k][2]++;
                else
                    // get to [-1,0] and go right
                    mr[k][1]--;
            }
        }
    }

    if (sdiv == 2)
    {
        mr[1][0] = mr[0][0];
        mr[1][1] = mr[0][1] + 1.;
        mr[1][2] = mr[0][2] + 1.;

        mr[2][0] = mr[0][0];
        mr[2][1] = mr[0][1] + 1.;
        mr[2][2] = mr[0][2] - 1.;

        mr[3][0] = mr[0][0];
        mr[3][1] = mr[0][1] + 1.;
        mr[3][2] = mr[0][2];

        mr[5][0] = mr[4][0];
        mr[5][1] = mr[4][1] + 1.;
        mr[5][2] = mr[4][2] + 1.;

        mr[6][0] = mr[4][0];
        mr[6][1] = mr[4][1] + 1.;
        mr[6][2] = mr[4][2] - 1.;

        mr[7][0] = mr[4][0];
        mr[7][1] = mr[4][1] + 1.;
        mr[7][2] = mr[4][2];
    }
    else
    {
        mr[1][0] = mr[0][0];
        mr[1][1] = mr[0][1] + 1.;
        mr[1][2] = mr[0][2];

        mr[2][0] = mr[0][0];
        mr[2][1] = mr[0][1];
        mr[2][2] = mr[0][2] + 1.;

        mr[3][0] = mr[0][0];
        mr[3][1] = mr[0][1] + 1.;
        mr[3][2] = mr[0][2] + 1.;

        mr[5][0] = mr[4][0];
        mr[5][1] = mr[4][1] + 1.;
        mr[5][2] = mr[4][2];

        mr[6][0] = mr[4][0];
        mr[6][1] = mr[4][1];
        mr[6][2] = mr[4][2] + 1.;

        mr[7][0] = mr[4][0];
        mr[7][1] = mr[4][1] + 1.;
        mr[7][2] = mr[4][2] + 1.;
    }

    if (sdiv == 4)  // search in bcc*2
        for (k = 0; k < 8; k++)
            for (j = 1; j < DIMENSION; j++)
                mr[k][j] *= 2;

    for (k = 0; k < 8; k++)
        for (j = 0; j < DIMENSION; j++)
        {
            mr[k][j] *= idiv;
            mr[k][j] = mr[k][j] < 0 ? 0. : (mr[k][j] > 255. ? 255. : mr[k][j]);
        }
}

double reconstruct_rnd(double  data[MAX_ENTRIES][DIMENSION],
                       int     numEntries,
                       int     index_[MAX_ENTRIES],
                       double  out[MAX_ENTRIES][DIMENSION],
                       int     ns,
                       double  direction[DIMENSION],
                       double* step)
{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
#define NP1 31

    static int code1[NP1][3] =  // step, mean div,diamond
        /*
        GOOD
        */
        {{7, 1, 2},   {11, 1, 2},  {15, 1, 2},  {19, 1, 4},  {24, 1, 4},  {29, 1, 4},  {34, 1, 4},  {40, 1, 4},  {46, 1, 4},  {53, 1, 4},  {60, 2, 1},
         {67, 2, 1},  {75, 2, 1},  {83, 2, 1},  {92, 2, 2},  {101, 2, 2}, {111, 2, 2}, {122, 2, 2}, {133, 2, 2}, {145, 2, 2}, {158, 2, 2}, {172, 2, 2},
         {187, 2, 2}, {204, 2, 4}, {223, 2, 4}, {245, 2, 4}, {270, 2, 4}, {301, 4, 1}, {339, 4, 1}, {392, 4, 1}, {448, 4, 1}};

#define NP2 7

    static int code2[NP2][3] =  // step, mean div,diamond
        {{7, 4, 2}, {13, 8, 1}, {26, 8, 1}, {48, 8, 1}, {79, 8, 2}, {111, 16, 1}, {158, 16, 1}};

    int dir_limit1 = 13;
    int dir_limit2 = 6;

    static int report[3] = {0, 0, 0};

    int(*code)[3];
    int dir_limit = dir_limit1;
    int np;
    //##############################
    if (ns == 3)
    {
        //##############################
        code      = code2;
        dir_limit = dir_limit2;
        np        = NP2;
    }
    else
    {
        code      = code1;
        dir_limit = dir_limit1;
        np        = NP1;
    }

    if (!report[ns])
    {
        report[ns] = 1;

        int bd = 1;
        int s, i;

        printf("Quantizer report, sets %d\n", ns);

        for (i = 0; i < np; i++)
        {
            int td = code[i][1] * code[i][1] * code[i][1] * code[i][2];
            if (td < bd)
                printf("non-monotonic diver codes?? was  %d  now  %d  at %d\n", bd, td, i);
            bd = td;
        }
        printf("Max divider %d \n", bd);
        for (s = i = 0; i < np; i++)
            s += bd / (code[i][1] * code[i][1] * code[i][1] * code[i][2]);
        printf("Soft estimate; position capacity (scaled by max divider) %d, direction %d \n", s, 24 * dir_limit * dir_limit + 2);

        for (s = i = 0; i < np; i++)
            s += bd / (code[i][1] * code[i][1] * code[i][1] * code[i][2]) *
                 (dir_limit < code[i][0] ? 24 * dir_limit * dir_limit + 2 : 24 * code[i][0] * code[i][0] + 2);

        printf("Tight estimate, all+divider %d, bits %f;\n sets left total %d, full sets left assuming full directions %d\n",
               s,
               log((double)s) / log(2.),
               (int)floor(pow(2., ceil(log((double)s) / log(2.))) + 0.5) - s,
               ((int)floor(pow(2., ceil(log((double)s) / log(2.))) + 0.5) - s) / (24 * dir_limit * dir_limit + 2)

        );
        double b = log((double)s) / log(2.) + 24;

        while (bd > 1)
        {
            b--;
            bd /= 2;
        }

        printf("Tight estimate, TOTAL  %g\n", b);
    }

    int           i, j, k;
    static double mean[DIMENSION];

    int ok;
    int maxTry = 10;

    static int index[MAX_ENTRIES];
    static int index_r[MAX_ENTRIES];

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

    double err = 1512 * 512 * 3 * 16;
    double er1;

    double out1[MAX_ENTRIES][DIMENSION];
    // expand/collapse if needed

    if (ns == 1)  // 3 bit indes
        index_expand(index, numEntries, 16);
    else
        index_expand(index, numEntries, MAX_CLUSTERS);

    PCnt += numEntries;

    do
    {
        double idx_mean;
        mds_d(data, numEntries, index, mean, &idx_mean, direction, step);

        double step_li = *step;

        static double dir_r[4][DIMENSION];

        static double dir_step_r[2][4][DIMENSION];  // alternative for dir_r

        static double mean_r[8][DIMENSION];
        static double mean_step_r[2][8][DIMENSION];
        static double step_r[2];

        if (step_li > 200000)
        {  // bypass {
            NShakeCnt += numEntries;
            for (i = 0; i < numEntries; i++)
                for (j = 0; j < DIMENSION; j++)
                    out[i][j] = mean[j] + direction[j] * (*step) * (index[i] - idx_mean);
            return totalError(data, out, numEntries);
        }
#define STEP_FACTOR 7.

        if (ns == 1)
        {
#define STEP_DIV 1.
#define MEAN_DIV 1.
            for (j = 0; j < DIMENSION; j++)
                mean_step_r[0][0][j] = mean_step_r[1][0][j] = mean_r[0][j] = floor(mean[j] / MEAN_DIV) * MEAN_DIV;

            int ii[3];

            // cartesian shake
            for (ii[0] = 0; ii[0] < 2; ii[0]++)
                for (ii[1] = 0; ii[1] < 2; ii[1]++)
                    for (ii[2] = 0; ii[2] < 2; ii[2]++)
                        for (j = 0; j < DIMENSION; j++)
                            mean_step_r[0][ii[0] + ii[1] * 2 + ii[2] * 4][j] = mean_step_r[1][ii[0] + ii[1] * 2 + ii[2] * 4][j] =
                                mean_r[ii[0] + ii[1] * 2 + ii[2] * 4][j]     = mean_r[0][j] + MEAN_DIV * ii[j] > 255 ? 255 : mean_r[0][j] + MEAN_DIV * ii[j];

            step_li = floor((step_li * STEP_FACTOR + .5));

            step_li = step_li > 256 * STEP_FACTOR ? 256 * STEP_FACTOR : step_li;

            int    mc = -1;
            double tt = 2;

            for (i = 0; i < DIMENSION; i++)
                if (fabs(fabs(direction[i]) - 1) < tt)
                {
                    tt = fabs(fabs(direction[i]) - 1);
                    mc = i;
                }
            if (mc < 0 && step_li != 0)
            {
                printf("dir mormalizing error");
            }

            for (i = 0; i < 2; i++)
                for (ii[0] = 0; ii[0] < 2; ii[0]++)
                    for (ii[1] = 0; ii[1] < 2; ii[1]++)
                        for (j = 0; j < DIMENSION; j++)
                            dir_step_r[i][ii[0] + 2 * ii[1]][j] = (j == mc)
                                                                      ? direction[j] * (step_li + i) / STEP_FACTOR
                                                                      : ((floor(direction[j] * (step_li + i))) + (j < mc ? ii[j] : ii[j - 1])) / STEP_FACTOR;
            shake_d_s(data, numEntries, idx_mean, dir_step_r, mean_r, index_r, out1);
        }
        else
        {
            // step quantizer in ramp
            for (i = 0; i < np; i++)
                if (step_li * STEP_FACTOR <= code[i][0])
                    break;
            if (i != 0)
                i--;
            if (i == np - 1)
                i--;

            //"low step"
            // bcc*2 and cartesian; starting points for fcc
            for (j = 0; j < 2; j++)
                setMean(mean, mean_step_r[j], code[i + j][1] * code[i + j][1] * code[i + j][1] * code[i + j][2]);

            step_li = code[i][0];

            double df[2];

            df[0] = code[i][0] < dir_limit ? code[i][0] : dir_limit;
            df[1] = code[i + 1][0] < dir_limit ? code[i + 1][0] : dir_limit;

            int    mc = -1;
            double tt = 2;

            for (j = 0; j < DIMENSION; j++)
                if (fabs(fabs(direction[j]) - 1) < tt)
                {
                    tt = fabs(fabs(direction[j]) - 1);
                    mc = j;
                }
            if (mc < 0 && step_li != 0)
            {
                printf("dir normalizing error");
            }

            int ii[3];
            for (k = 0; k < 2; k++)
                for (ii[0] = 0; ii[0] < 2; ii[0]++)
                    for (ii[1] = 0; ii[1] < 2; ii[1]++)
                        for (j = 0; j < DIMENSION; j++)
                            dir_step_r[k][ii[0] + 2 * ii[1]][j] =
                                (j == mc) ? direction[j] * (double)code[i + k][0] / STEP_FACTOR
                                          : (floor(direction[j] * df[k]) + (j < mc ? ii[j] : ii[j - 1])) / df[k] * (double)code[i + k][0] / STEP_FACTOR;
            shake_d_s_s(data, numEntries, MAX_CLUSTERS, idx_mean, dir_step_r, mean_step_r, index_r, out1);
        }

        er1 = totalError(data, out1, numEntries);

        ok = 1;

        if (er1 < err)
        {
            for (i = 0; i < numEntries; i++)
                ok = ok && (index[i] == index_r[i]);

            for (i = 0; i < numEntries; i++)
                index[i] = index_r[i];

            for (i = 0; i < numEntries; i++)
                for (j = 0; j < DIMENSION; j++)
                    out[i][j] = out1[i][j];
            err = er1;
        }
    } while (!ok && maxTry-- > 0);

    return totalError(data, out, numEntries);
};

double reconstruct_rnd_mean_clip(double  data[MAX_ENTRIES][DIMENSION],
                                 int     numEntries,
                                 int     index_[MAX_ENTRIES],
                                 double  out[MAX_ENTRIES][DIMENSION],
                                 int     ns,
                                 double  direction[DIMENSION],
                                 double* step,
                                 double  in_mean[DIMENSION])
{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
#define NP1 31

    static int code1[NP1][3] =  // step, mean div,diamond
        /*
        GOOD
        */
        {{7, 1, 2},   {11, 1, 2},  {15, 1, 2},  {19, 1, 4},  {24, 1, 4},  {29, 1, 4},  {34, 1, 4},  {40, 1, 4},  {46, 1, 4},  {53, 1, 4},  {60, 2, 1},
         {67, 2, 1},  {75, 2, 1},  {83, 2, 1},  {92, 2, 2},  {101, 2, 2}, {111, 2, 2}, {122, 2, 2}, {133, 2, 2}, {145, 2, 2}, {158, 2, 2}, {172, 2, 2},
         {187, 2, 2}, {204, 2, 4}, {223, 2, 4}, {245, 2, 4}, {270, 2, 4}, {301, 4, 1}, {339, 4, 1}, {392, 4, 1}, {448, 4, 1}};

#undef NP2
#define NP2 8

    static int code2[NP2][3] =  // step, mean div,diamond

        {{7, 8, 1}, {24, 8, 1}, {46, 8, 1}, {75, 8, 1}, {111, 8, 1}, {158, 8, 1}, {223, 8, 1}, {448, 8, 1}};

    int dir_limit1 = 13;
    int dir_limit2 = 6;

    static int report[3] = {0, 0, 0};

    int(*code)[3];
    int dir_limit = dir_limit1;
    int np;

    if (ns == 3)
    {
        code      = code2;
        dir_limit = dir_limit2;
        np        = NP2;
    }
    else
    {
        code      = code1;
        dir_limit = dir_limit1;
        np        = NP1;
    }

    if (!report[ns])
    {
        report[ns] = 1;

        int bd = 1;
        int s, i;

        printf("Quantizer report, sets %d\n", ns);

        for (i = 0; i < np; i++)
        {
            int td = code[i][1] * code[i][1] * code[i][1] * code[i][2];
            if (td < bd)
                printf("non-monotonic diver codes?? was  %d  now  %d  at %d\n", bd, td, i);
            bd = td;
        }
        printf("Max divider %d \n", bd);
        for (s = i = 0; i < np; i++)
            s += bd / (code[i][1] * code[i][1] * code[i][1] * code[i][2]);
        printf("Soft estimate; position capacity (scaled by max divider) %d, direction %d \n", s, 24 * dir_limit * dir_limit + 2);

        for (s = i = 0; i < np; i++)
            s += bd / (code[i][1] * code[i][1] * code[i][1] * code[i][2]) *
                 (dir_limit < code[i][0] ? 24 * dir_limit * dir_limit + 2 : 24 * code[i][0] * code[i][0] + 2);

        printf("Tight estimate, all+divider %d, bits %f;\n sets left total %d, full sets left assuming full directions %d\n",
               s,
               log((double)s) / log(2.),
               (int)floor(pow(2., ceil(log((double)s) / log(2.))) + 0.5) - s,
               ((int)floor(pow(2., ceil(log((double)s) / log(2.))) + 0.5) - s) / (24 * dir_limit * dir_limit + 2)

        );
        double b = log((double)s) / log(2.) + 24;

        while (bd > 1)
        {
            b--;
            bd /= 2;
        }

        printf("Tight estimate, TOTAL  %g\n", b);
    }

    int           i, j, k;
    static double mean[DIMENSION];

    int ok;
    int maxTry = 10;

    static int index[MAX_ENTRIES];
    static int index_r[MAX_ENTRIES];

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

    double err = 1512 * 512 * 3 * 16;
    double er1;

    double out1[MAX_ENTRIES][DIMENSION];
    // expand/collapse if needed

    if (ns == 1)  // 3 bit indes
        index_expand(index, numEntries, 16);
    else
        index_expand(index, numEntries, MAX_CLUSTERS);

    PCnt += numEntries;

    do
    {
        double idx_mean;

        mds(data, numEntries, index, mean, &idx_mean, direction, step);

        double s = 0, t = 0, q = 0, q1 = 0;

        for (j = 0; j < DIMENSION; j++)
        {
            for (mean[j] = k = 0; k < numEntries; k++)
                mean[j] += data[k][j];
            mean[j] /= (double)numEntries;
        }

        for (k = 0; k < numEntries; k++)
        {
            s += index[k];
            t += index[k] * index[k];
        }

        for (j = 0; j < DIMENSION; j++)
        {
            direction[j] = 0;
            for (k = 0; k < numEntries; k++)
                direction[j] += (data[k][j] - mean[j]) * index[k];

            q += direction[j] * direction[j];
            q1 = fabs(direction[j]) > q1 ? fabs(direction[j]) : q1;
        }

        s /= (double)numEntries;

        t = t - s * s * (double)numEntries;

        t = (t == 0 ? 0. : 1 / t);

        q = sqrt(q);

        *step = t * q;

        for (j = 0; j < DIMENSION; j++)
        {
            direction[j] /= (q1 == 0 ? 1 : q1);
            if (direction[j] >= 1.0001)
                printf("error\n");
        }

        if (*step != 0)
            *step *= q1 / q;

        double step_li = *step;

        static double dir_r[4][DIMENSION];

        static double dir_step_r[2][4][DIMENSION];  // alternative for dir_r

        static double mean_r[8][DIMENSION];
        static double mean_step_r[2][8][DIMENSION];
        static double step_r[2];

        if (step_li > 200000)
        {  // bypass {
            NShakeCnt += numEntries;
            for (i = 0; i < numEntries; i++)
                for (j = 0; j < DIMENSION; j++)
                    out[i][j] = mean[j] + direction[j] * (*step) * (index[i] - idx_mean);
            return totalError(data, out, numEntries);
        }
#define STEP_FACTOR 7.

        if (ns == 1)
        {
#define STEP_DIV 1.
#define MEAN_DIV 1.
            for (j = 0; j < DIMENSION; j++)
                mean_step_r[0][0][j] = mean_step_r[1][0][j] = mean_r[0][j] = floor(mean[j] / MEAN_DIV) * MEAN_DIV;

            int ii[3];

            // cartesian shake
            for (ii[0] = 0; ii[0] < 2; ii[0]++)
                for (ii[1] = 0; ii[1] < 2; ii[1]++)
                    for (ii[2] = 0; ii[2] < 2; ii[2]++)
                        for (j = 0; j < DIMENSION; j++)
                            mean_step_r[0][ii[0] + ii[1] * 2 + ii[2] * 4][j] = mean_step_r[1][ii[0] + ii[1] * 2 + ii[2] * 4][j] =
                                mean_r[ii[0] + ii[1] * 2 + ii[2] * 4][j]     = mean_r[0][j] + MEAN_DIV * ii[j] > 255 ? 255 : mean_r[0][j] + MEAN_DIV * ii[j];

            step_li = floor((step_li * STEP_FACTOR - STEP_FACTOR) / STEP_DIV) * STEP_DIV + STEP_FACTOR;

            step_li = step_li < STEP_FACTOR ? STEP_FACTOR : step_li;

            step_li = step_li > 256 ? 256 : step_li;

            int    mc = -1;
            double tt = 2;

            for (i = 0; i < DIMENSION; i++)
                if (fabs(fabs(direction[i]) - 1) < tt)
                {
                    tt = fabs(fabs(direction[i]) - 1);
                    mc = i;
                }
            if (mc < 0 && step_li != 0)
            {
                printf("dir mormalizing error");
            }

            for (i = 0; i < 2; i++)
                for (ii[0] = 0; ii[0] < 2; ii[0]++)
                    for (ii[1] = 0; ii[1] < 2; ii[1]++)
                        for (j = 0; j < DIMENSION; j++)
                            dir_step_r[i][ii[0] + 2 * ii[1]][j] =
                                (j == mc) ? direction[j] * (step_li + STEP_DIV * i) / STEP_FACTOR
                                          : ((floor(direction[j] * (step_li + STEP_DIV * i))) + (j < mc ? ii[j] : ii[j - 1])) / STEP_FACTOR;
            shake_d_s_s(data, numEntries, MAX_CLUSTERS, idx_mean, dir_step_r, mean_step_r, index_r, out1);
        }
        else
        {
            // step quantizer in ramp
            for (i = 0; i < np; i++)
                if (step_li * STEP_FACTOR <= code[i][0])
                    break;
            if (i != 0)
                i--;
            if (i == np - 1)
                i--;

            //"low step"

            // bcc*2 and cartesian; starting points for fcc
            for (j = 0; j < 2; j++)
            {
                for (j = 0; j < DIMENSION; j++)
                    mean[j] -= in_mean[j];

                setMean(mean, mean_step_r[j], code[i + j][1] * code[i + j][1] * code[i + j][1] * code[i + j][2]);

                for (k = 0; k < 8; k++)
                    for (j = 0; j < DIMENSION; j++)
                        mean_step_r[j][k][j] += in_mean[j];
            }

            step_li = code[i][0];

            double df[2];

            df[0] = code[i][0] < dir_limit ? code[i][0] : dir_limit;
            df[1] = code[i + 1][0] < dir_limit ? code[i + 1][0] : dir_limit;

            int    mc = -1;
            double tt = 2;

            for (j = 0; j < DIMENSION; j++)
                if (fabs(fabs(direction[j]) - 1) < tt)
                {
                    tt = fabs(fabs(direction[j]) - 1);
                    mc = j;
                }
            if (mc < 0 && step_li != 0)
            {
                printf("dir normalizing error");
            }

            int ii[3];
            for (k = 0; k < 2; k++)
                for (ii[0] = 0; ii[0] < 2; ii[0]++)
                    for (ii[1] = 0; ii[1] < 2; ii[1]++)
                        for (j = 0; j < DIMENSION; j++)
                            dir_step_r[k][ii[0] + 2 * ii[1]][j] =
                                (j == mc) ? direction[j] * (double)code[i + k][0] / STEP_FACTOR
                                          : (floor(direction[j] * df[k]) + (j < mc ? ii[j] : ii[j - 1])) / df[k] * (double)code[i + k][0] / STEP_FACTOR;

            shake_d_s_s(data, numEntries, MAX_CLUSTERS, idx_mean, dir_step_r, mean_step_r, index_r, out1);
        }

        er1 = totalError(data, out1, numEntries);

        ok = 1;

        if (er1 < err)
        {
            for (i = 0; i < numEntries; i++)
                ok = ok && (index[i] == index_r[i]);

            for (i = 0; i < numEntries; i++)
                index[i] = index_r[i];

            for (i = 0; i < numEntries; i++)
                for (j = 0; j < DIMENSION; j++)
                    out[i][j] = out1[i][j];
            err = er1;
        }
    } while (!ok && maxTry-- > 0);

    return totalError(data, out, numEntries);
}

inline int getns(int partition[MAX_ENTRIES], int numEntries)
{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    int i, c;
    int id[MAX_ENTRIES];

    for (i = 0; i < MAX_SUBSETS; i++)
        id[i] = 0;

    for (i = 0; i < numEntries; i++)
        id[partition[i]] = 1;

    for (c = i = 0; i < MAX_SUBSETS; i++)
        c += (id[i] != 0);

    return (c);
}

int block_mean_rnd(double data_[MAX_ENTRIES][DIMENSION],
                   int    numEntries,
                   int    partition[MAX_ENTRIES],
                   int    ns,

                   double  mean[MAX_SUBSETS][DIMENSION],
                   double* clip,
                   double  mm[DIMENSION],
                   double  ni[MAX_SUBSETS]  // norm

)
{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    int i, j, k, l;

    double n[MAX_SUBSETS];  // number per set

    if (ns != 3)
        printf("ns problems\n");

    for (i = 0; i < ns; i++)
    {
        n[i] = 0;
        for (j = 0; j < DIMENSION; j++)
            mean[i][j] = 0;
    }

    //    set index compaction
    for (i = 0; i < numEntries; i++)
    {
        int ptr = partition[i];
        for (j = 0; j < DIMENSION; j++)
        {
            mean[ptr][j] += data_[i][j];
        }
        n[ptr]++;
    };

    for (j = 0; j < DIMENSION; j++)
    {
        mm[j] = 0;
        for (i = 0; i < ns; i++)
        {
            mm[j] += mean[i][j];
        }
        mm[j] /= (double)numEntries;
    }

    for (i = 0; i < ns; i++)
    {
        for (j = 0; j < DIMENSION; j++)
            if (n[i] != 0)
                mean[i][j] /= (double)n[i];
    }

    for (l = 0; l < ns; l++)
    {
        ni[l] = 0;
        for (k = 0; k < DIMENSION; k++)
        {
            ni[l] = ni[l] > fabs(mean[l][k] - mean[(l + 1) % 3][k]) ? ni[l] : fabs(mean[l][k] - mean[(l + 1) % 3][k]);
        }
    };

    for (l = 0; l < ns; l++)
        for (i = l + 1; i < ns; i++)
        {
            if (ni[i] < ni[l])
            {
                double t = ni[l];
                ni[l]    = ni[i];
                ni[i]    = t;
            }
        }

    for (j = 0; j < DIMENSION; j++)
    {
        for (i = 0; i < ns; i++)
        {
            mean[i][j] = floor(mean[i][j] / 8) * 8.;
        }
        mm[j] = floor(mm[j] / 8) * 8.;
    }

    *clip = 8.;
    return (ni[1] < 16.);
}

double ep_shaker(
#define EP_CLUMP

    double  data[MAX_ENTRIES][DIMENSION],
    int     numEntries,
    int     index_[MAX_ENTRIES],
    double  out[MAX_ENTRIES][DIMENSION],
    int     ns,
    double  direction[DIMENSION],
    double* step,
    int     lock)

{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    double s = 0, t = 0;
    int    i, j, k;
    double mean[DIMENSION];
    int    index[MAX_ENTRIES];

    int maxTry = 5;

    int Mi = 0;

    int Mi_ = (ns == 1 ? 16 : (ns == 2 ? 8 : 8));
    ;

    int bits;

    bits = (ns == 1 ? 0 : (ns == 2 ? 2 : 4));

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

    for (k = 0; k < numEntries; k++)
        if (index[k] >= Mi_)
        {
            printf("index error\n");
            for (i = 0; i < numEntries; i++)
                index[i] = 0;
            // work-around for constant block quantizer bug
            break;
        }

    // run on actually used exreme points
    index_collapse(index, numEntries);

    int done;
    int change;
    int start = 1;
    int better;

    double of = 0.0;

    double m[DIMENSION];
    double M[DIMENSION];

    double mb[DIMENSION];
    double Mb[DIMENSION];

    do
    {
        for (j = 0; j < DIMENSION; j++)
        {
            for (mean[j] = k = 0; k < numEntries; k++)
                mean[j] += data[k][j];
            mean[j] /= (double)numEntries;
        }

        if (lock)
            Mi = Mi_ - 1;
        else
            for (k = 0; k < numEntries; k++)
                Mi = index[k] > Mi ? index[k] : Mi;

        for (k = 0; k < numEntries; k++)
        {
            s += index[k];
            t += index[k] * index[k];
        }

        for (j = 0; j < DIMENSION; j++)
        {
            direction[j] = 0;
            for (k = 0; k < numEntries; k++)
                direction[j] += (data[k][j] - mean[j]) * index[k];
        }

        s /= (double)numEntries;
        t     = t - s * s * (double)numEntries;
        *step = t = (t == 0 ? 0. : 1 / t);

        for (j = 0; j < DIMENSION; j++)
        {
            m[j] = floor((mean[j] + direction[j] * (*step) * (0 - s)) / ((double)(1 << bits))) * (1 << bits);
#ifdef EP_CLUMP
            m[j] = m[j] > 0 ? m[j] : 0;
            m[j] = m[j] < 255. ? m[j] : 255.;
#endif
            M[j] = floor((mean[j] + direction[j] * (*step) * (Mi - s)) / ((double)(1 << bits))) * (1 << bits);
#ifdef EP_CLUMP
            M[j] = M[j] > 0 ? M[j] : 0;
            M[j] = M[j] < 255. ? M[j] : 255.;
#endif
        }

        int glue = 1;
        for (j = 0; j < DIMENSION; j++)
            glue = glue && (m[j] == M[j]);

        if (glue)
        {
            if (start)
            {
                for (i = 0; i < numEntries; i++)
                    for (j = 0; j < DIMENSION; j++)
                        out[i][j] = m[j];
                return totalError(data, out, numEntries);
            }
            else
                return totalError(data, out, numEntries);
        }

        double pp[64][DIMENSION];
        double c[MAX_ENTRIES][64][DIMENSION];

        for (i = 0; i <= Mi; i++)
            for (j = 0; j < DIMENSION; j++)
            {
                pp[i][j] = floor(m[j] + (M[j] - m[j]) * (i / (double)Mi) + 0.5);
                pp[i][j] = pp[i][j] > 0 ? pp[i][j] : 0;
                pp[i][j] = pp[i][j] < 255. ? pp[i][j] : 255.;
            }

        for (k = 0; k < numEntries; k++)
            for (i = 0; i <= Mi; i++)
                for (j = 0; j < DIMENSION; j++)
                {
                    double t1  = (data[k][j] - pp[i][j]);
                    c[k][i][j] = t1 * t1;
                }

        double outg[MAX_ENTRIES][DIMENSION];
        int    idg[MAX_ENTRIES];
        int    id[MAX_ENTRIES];

        double mag = 0;
        double ma  = 0;

        for (k = 0; k < numEntries; k++)
        {
            idg[k]    = 0;
            double t1 = c[k][0][0] + c[k][0][1] + c[k][0][2];
            for (i = 1; i <= Mi; i++)
            {
                double s1 = c[k][i][0] + c[k][i][1] + c[k][i][2];
                if (s1 < t1)
                {
                    t1     = s1;
                    idg[k] = i;
                }
            }
            ma += t1;
        }
        mag = ma;
        for (k = 0; k < numEntries; k++)
        {
            for (j = 0; j < DIMENSION; j++)
                outg[k][j] = pp[idg[k]][j];
        }

        int ss = 0;
        int p, q, r;

        for (p = 1; p < 64; p++)
        {
            // Gray code increment bit
            q = p ^ (p >> 1) ^ (p - 1) ^ ((p - 1) >> 1);
            r = p & (-p);
            if (q != r)
                printf("Gray code problem");

            int di;

            di     = (ss & q) ? -(1 << bits) : (1 << bits);
            ss     = ss ^ q;
            int i0 = 0, j0 = 0;
            if (q >= 8)
            {
                i0++;
                q >>= 3;
            };
            for (q >>= 1; q != 0; q >>= 1)
                j0++;

            if (i0 == 0)
            {
                m[j0] += di;
#ifdef EP_CLUMP
                m[j0] = m[j0] > 0 ? m[j0] : 0;
                m[j0] = m[j0] < 255. ? m[j0] : 255.;
#endif
            }
            else
            {
                M[j0] += di;
#ifdef EP_CLUMP
                M[j0] = M[j0] > 0 ? M[j0] : 0;
                M[j0] = M[j0] < 255. ? M[j0] : 255.;
#endif
            }

            for (i = 0; i <= Mi; i++)
            {
                pp[i][j0] = floor(m[j0] + (M[j0] - m[j0]) * (i / (double)Mi) + 0.5);
                pp[i][j0] = pp[i][j0] > 0 ? pp[i][j0] : 0;
                pp[i][j0] = pp[i][j0] < 255. ? pp[i][j0] : 255.;
            }

            for (k = 0; k < numEntries; k++)
                for (i = 0; i <= Mi; i++)
                {
                    double t1   = (data[k][j0] - pp[i][j0]);
                    c[k][i][j0] = t1 * t1;
                }

            ma = 0;

            for (k = 0; k < numEntries; k++)
            {
                id[k]     = 0;
                double t1 = c[k][0][0] + c[k][0][1] + c[k][0][2];
                for (i = 1; i <= Mi; i++)
                {
                    double s1 = c[k][i][0] + c[k][i][1] + c[k][i][2];
                    if (s1 < t1)
                    {
                        t     = s1;
                        id[k] = i;
                    }
                }
                ma += t1;
            }
            if (ma < mag)
            {
                mag = ma;
                for (k = 0; k < numEntries; k++)
                {
                    idg[k] = id[k];
                    for (j = 0; j < DIMENSION; j++)
                    {
                        outg[k][j] = pp[idg[k]][j];
                        mb[j]      = m[j];
                        Mb[j]      = M[j];
                    }
                }
            }
        }
        double nf = totalError(data, outg, numEntries);

        change = 0;
        for (k = 0; k < numEntries; k++)
        {
            change = change || (index[k] != idg[k]);
        }
        if (!start)
        {
            better = nf < of;
        }
        else
            better = 1;

        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];
            }
            of = nf;
        }
        done = !(change && better);
    } while (!done && maxTry--);
    return totalError(data, out, numEntries);
}

#define SIZE 6

int expand(int bits, int v)
{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    return (v << (8 - bits) | v >> (2 * bits - 8));
}

void getRndRamp(int bits, double v, int size, double out[], int* parity, int range[2], int ep[])
{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif

    int m;
    m = (int)floor(v / (double)(1 << (8 - bits)));
    m = m < 0 ? 0 : m;
    m = m > (1 << bits) - 1 ? (1 << bits) - 1 : m;

    while (expand(bits, m + 1) < v && m < (1 << bits) - 1)
        m++;

    m -= ((size + 1) / 2 - 1);  // normally size is even, save size == 1

    *parity = m & 0x1;

    int i;
    range[0] = 0;
    range[1] = size;

    for (i = 0; i < size; i++)
    {
        if (m >= 0)
        {
            if (m < (1 << bits))
            {
                out[i] = m;  // no expansion here, as single point bcc handling is outside
                ep[i]  = m;
            }
            else if (range[1] > i)
            {
                range[1] = i;
            }
        }
        else
        {
            range[0] = i + 1;
        }
        m++;
    }
}

void getRndRampN(int    bits[DIMENSION],
                 double v[DIMENSION],
                 int    size,
                 double out[MAX_SHAKE_SIZE][DIMENSION],
                 int    parity[DIMENSION],
                 int    range[2][DIMENSION],
                 int    ep[MAX_SHAKE_SIZE][DIMENSION],
                 int    bcc)
{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
    static double out_[DIMENSION][MAX_SHAKE_SIZE];
    int           range_[DIMENSION][2];
    int           ep_[DIMENSION][MAX_SHAKE_SIZE];

    int i, j;
    for (j = 0; j < DIMENSION; j++)
        getRndRamp(bits[j] + bcc, v[j], size, out_[j], parity + j, range_[j], ep_[j]);
    //
    // If bcc, we expand bits by 1, quantizer should take care about partity
    //
    if (size > 1)
    {
        for (j = 0; j < DIMENSION; j++)
        {
            for (i = 0; i < size; i++)
                out[i][j] = expand(bits[j] + bcc, (int)out_[j][i]);
            for (i = 0; i < 2; i++)
                range[i][j] = range_[j][i];
            for (i = 0; i < size; i++)
                ep[i][j] = ep_[j][i];
        }
    }
    else if (size == 1)
    {  // m in range
        for (j = 0; j < DIMENSION; j++)
        {
            range[0][j] = 0;
            range[1][j] = 1;
        }
        if (bcc)
        {
            double t0 = 0, t1 = 0;
            int    ok0 = 1, ok1 = 1;
            int    p = 0;

            for (j = 0; j < DIMENSION; j++)
                out_[j][1] = out_[j][0] + 1;
            for (j = 0; j < DIMENSION; j++)
            {
                // partity 0
                t0 += (expand(bits[j] + bcc, (int)(out_[j][parity[j]] - v[j]))) * (expand(bits[j] + bcc, (int)(out_[j][parity[j]] - v[j])));
                ok0 = ok0 && out_[j][parity[j]] < ((1 << (bits[j] + bcc)));
                // partity 0
                t1 += (expand(bits[j] + bcc, (int)(out_[j][(parity[j] ^ 1) & 1] - v[j]))) * (expand(bits[j] + bcc, (int)(out_[j][(parity[j] ^ 1) & 1] - v[j])));

                ok1 = ok1 && out_[j][(parity[j] ^ 1) & 1] < ((1 << (bits[j] + bcc)));
            }
            if (ok0 && ok1)
                if (t0 < t1)
                    p = 0;
                else
                    p = 1;
            else if (ok0)
                p = 0;
            else if (ok1)
                p = 1;
            else
                printf(
                    "problems with single point bcc clumping %g %g  %g %g  %g %g \n", out_[0][0], out_[0][1], out_[1][0], out_[1][1], out_[2][0], out_[2][1]);
            for (j = 0; j < DIMENSION; j++)
            {
                out[0][j] = expand(bits[j] + bcc, (int)(out_[j][(parity[j] ^ p) & 1]));
                ep[0][j]  = (int)out_[j][(parity[j] ^ p) & 1];
            }
        }
        else
        {
            for (j = 0; j < DIMENSION; j++)
            {
                if (out_[j][0] + 1 < ((1 << (bits[j]))) &&  // bcc == 0
                    (abs(expand(bits[j] + bcc, (int)(out_[j][0] + 1 - v[j]))) < abs(expand(bits[j] + bcc, (int)(out_[j][0] - v[j])))))
                {
                    out[0][j] = expand(bits[j] + bcc, (int)(out_[j][0] + 1));
                    ep[0][j]  = (int)(out_[j][0] + 1);
                }
                else
                {
                    out[0][j] = expand(bits[j] + bcc, (int)(out_[j][0]));
                    ep[0][j]  = (int)(out_[j][0]);
                }
            }
        }
    }
    else
        printf("problems with size %d\n", size);
}

double ep_shaker_2(

    double data[MAX_ENTRIES][DIMENSION],
    int    numEntries,
    int    index_[MAX_ENTRIES],
    double out[MAX_ENTRIES][DIMENSION],
    int    epo_code[2][DIMENSION],
    int    bits[3],
    int    bcc,
    int    nClusters,
    int    size)
{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif

    int    i, j, k;
    double mean[DIMENSION];
    int    index[MAX_ENTRIES];

    int maxTry = 8;

    int Mi = 0;

    int Mi_ = nClusters - 1;  // this is last cluster, not max number 0f clusters

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

    for (k = 0; k < numEntries; k++)
        if (index[k] > Mi_)
        {
            printf("index error\n");
            for (i = 0; i < numEntries; i++)
                index[i] = 0;
            // work-around for constant block quantizer bug
            break;
        }

    index_collapse(index, numEntries);

    int done;
    int change;
    int start = 1;
    int better;

    double of = 0.0;
    double epo[2][DIMENSION];
    int    ep[2][MAX_SHAKE_SIZE][DIMENSION];
    int    parity[2][DIMENSION];

    int    range[2][2][DIMENSION];
    double epd[2][MAX_SHAKE_SIZE][DIMENSION];

    do
    {
        for (j = 0; j < DIMENSION; j++)
        {
            for (mean[j] = k = 0; k < numEntries; k++)
                mean[j] += data[k][j];
            mean[j] /= (double)numEntries;
        }

        for (k = 0; k < numEntries; k++)
            Mi = index[k] > Mi ? index[k] : Mi;

        // shift/scale

        int p, q;

        int p0 = -1, q0 = -1;

        double ee = MAX_ENTRIES * 256. * 256. * DIMENSION * 8.;  // 8 = slack

        if (Mi == 0)
        {
            getRndRampN(bits, mean, 1 /* size*/, epo + 0, parity[0], range[0], ep[0], bcc);
            getRndRampN(bits, mean, 1 /* size*/, epo + 1, parity[1], range[1], ep[1], bcc);
            for (j = 0; j < DIMENSION; j++)
            {
                epo_code[0][j] = ep[0][0][j];
                epo_code[1][j] = ep[0][0][j];
            }
        }

        for (q = 1; Mi != 0 && q * Mi <= Mi_; q++)
            for (p = q * Mi; p <= Mi_; p++)
            {
                //index * q + p-Mi*q

                int cidx[MAX_ENTRIES];

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

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

                // get ideal clustr centers
                double cc[MAX_CLUSTERS_BIG][DIMENSION];
                double cnt[MAX_CLUSTERS_BIG];

                for (j = 0; j < DIMENSION; j++)
                {
                    for (i = 0; i <= Mi_; i++)
                        cnt[i] = cc[i][j] = 0;
                    rp[0][j] = rp[1][j] = 0;
                }

                for (k = 0; k < numEntries; k++)
                {
                    cnt[cidx[k]] += 1;
                    for (j = 0; j < DIMENSION; j++)
                        cc[cidx[k]][j] += data[k][j];
                }

                for (i = 0; i <= Mi_; i++)
                    for (j = 0; j < DIMENSION; j++)
                        if (cnt[i] != 0)
                            cc[i][j] = floor(cc[i][j] / cnt[i] + 0.5);  // more or less ideal location
                        else
                            cc[i][j] = 0;

                // for cluster centers

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

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

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

                for (i = 0; i < 2; i++)
                    for (j = 0; j < DIMENSION; j++)
                    {
                        range[i][0][j] = 0;
                        range[i][1][j] = size;
                    }

                if (dd == 0)
                {
                    //############## degenerate, usually the same index
                    for (i = 0; i <= Mi_; i++)
                        if (cnt[i] != 0)
                            break;
                    // Make it into ramp
                    for (j = 0; j < DIMENSION; j++)
                    {
                        epa[0][j] = cc[i][j];
                        epa[1][j] = cc[i][j];
                    }
                    getRndRampN(bits, epa[0], size, epd[0], parity[0], range[0], ep[0], bcc);
                    getRndRampN(bits, epa[1], size, epd[1], parity[1], range[1], ep[1], bcc);
                }
                else
                {
                    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[1][0] * rp[0][j] + im[1][1] * rp[1][j]) * Mi_;
                        epa[1][j] = (im[0][0] * rp[0][j] + im[0][1] * rp[1][j]) * Mi_;
                    }
                    getRndRampN(bits, epa[0], size, epd[0], parity[0], range[0], ep[0], bcc);
                    getRndRampN(bits, epa[1], size, epd[1], parity[1], range[1], ep[1], bcc);
                }
                int    best_ep[2][2][2][DIMENSION];  // point, parity0,parity1, coordinate
                double e[2][2][DIMENSION];           //  parity0, parity1, coordinate
                int    l0, l1;

                for (j = 0; j < DIMENSION; j++)
                {
                    e[0][0][j] = e[0][1][j] = e[1][0][j] = e[1][1][j] = MAX_ENTRIES * 256 * 256 * 4.;

                    for (l0 = range[0][0][j]; l0 < range[0][1][j]; l0++)
                        for (l1 = range[1][0][j]; l1 < range[1][1][j]; l1++)
                        {
                            // ran on points or clusters ??
                            double cv[MAX_CLUSTERS_BIG];
                            double t = 0;
                            int    i1;
                            for (i1 = 0; i1 <= Mi_; i1++)
                            {
                                cv[i1] = floor((epd[1][l1][j] * i1 + epd[0][l0][j] * (Mi_ - i1)) / ((double)Mi_) + 0.5);
                                cv[i1] = cv[i1] < 0 ? 0 : (cv[i1] > 255 ? 255 : cv[i1]);
                            }

                            for (i1 = 0; i1 < numEntries; i1++)
                                t += (cv[cidx[i1]] - data[i1][j]) * (cv[cidx[i1]] - data[i1][j]);
                            if (bcc)
                            {
                                if (t < e[(parity[0][j] ^ l0) & 1][(parity[1][j] ^ l1) & 1][j])
                                {
                                    e[(parity[0][j] ^ l0) & 1][(parity[1][j] ^ l1) & 1][j]          = t;
                                    best_ep[0][(parity[0][j] ^ l0) & 1][(parity[1][j] ^ l1) & 1][j] = l0;
                                    best_ep[1][(parity[0][j] ^ l0) & 1][(parity[1][j] ^ l1) & 1][j] = l1;
                                }
                            }
                            else
                            {
                                if (t < e[0][0][j])
                                {
                                    e[0][0][j]          = t;
                                    best_ep[0][0][0][j] = l0;
                                    best_ep[1][0][0][j] = l1;
                                }
                            }
                        }
                }
                if (bcc)
                {
                    double t[2][2] = {0, 0, 0, 0};
                    double tt;
                    int    a, b, a0, b0;
                    for (a = 0; a < 2; a++)
                        for (b = 0; b < 2; b++)
                            for (j = 0; j < DIMENSION; j++)
                                t[a][b] += e[a][b][j];
                    a0 = b0 = 0;
                    tt      = t[0][0];
                    for (a = 0; a < 2; a++)
                        for (b = 0; b < 2; b++)
                            if (tt > t[a][b])
                            {
                                a0 = a;
                                b0 = b;
                                tt = t[a][b];
                            }

                    if (tt < ee)
                    {
                        ee = tt;
                        p0 = p;
                        q0 = q;
                        for (j = 0; j < DIMENSION; j++)
                        {
                            epo[0][j]      = epd[0][best_ep[0][a0][b0][j]][j];
                            epo[1][j]      = epd[1][best_ep[1][a0][b0][j]][j];
                            epo_code[0][j] = ep[0][best_ep[0][a0][b0][j]][j];
                            epo_code[1][j] = ep[1][best_ep[1][a0][b0][j]][j];
                        }
                    }
                }
                else
                {
                    double t = 0;
                    for (j = 0; j < DIMENSION; j++)
                        t += e[0][0][j];
                    if (t < ee)
                    {
                        ee = t;
                        p0 = p;
                        q0 = q;
                        for (j = 0; j < DIMENSION; j++)
                        {
                            epo[0][j]      = epd[0][best_ep[0][0][0][j]][j];
                            epo[1][j]      = epd[1][best_ep[1][0][0][j]][j];
                            epo_code[0][j] = ep[0][best_ep[0][0][0][j]][j];
                            epo_code[1][j] = ep[1][best_ep[1][0][0][j]][j];
                        }
                    }
                }
            }
        // reconstruct new ep

        // requantize
        // #############

        double pp[64][DIMENSION];
        double c[MAX_ENTRIES][64][DIMENSION];

        for (i = 0; i <= Mi_; i++)
            for (j = 0; j < DIMENSION; j++)
            {
                pp[i][j] = floor(epo[0][j] + (epo[1][j] - epo[0][j]) * (i / (double)Mi_) + 0.5);
                pp[i][j] = pp[i][j] > 0 ? pp[i][j] : 0;
                pp[i][j] = pp[i][j] < 255. ? pp[i][j] : 255.;
            }

        for (k = 0; k < numEntries; k++)
            for (i = 0; i <= Mi_; i++)
                for (j = 0; j < DIMENSION; j++)
                {
                    double t   = (data[k][j] - pp[i][j]);
                    c[k][i][j] = t * t;
                }

        double outg[MAX_ENTRIES][DIMENSION];
        int    idg[MAX_ENTRIES];

        double mag = 0;
        double ma  = 0;

        for (k = 0; k < numEntries; k++)
        {
            idg[k]   = 0;
            double t = c[k][0][0] + c[k][0][1] + c[k][0][2];
            for (i = 1; i <= Mi_; i++)
            {
                double s = c[k][i][0] + c[k][i][1] + c[k][i][2];
                if (s < t)
                {
                    t      = s;
                    idg[k] = i;
                }
            }
            ma += t;
        }
        mag = ma;
        for (k = 0; k < numEntries; k++)
        {
            for (j = 0; j < DIMENSION; j++)
                outg[k][j] = pp[idg[k]][j];
        }

        // change/better

        double nf = totalError(data, outg, numEntries);

        change = 0;
        for (k = 0; k < numEntries; k++)
        {
            change = change || (index[k] * q0 + p0 - Mi * q0 != idg[k]);
        }
        if (!start)
        {
            better = nf < of;
        }
        else
            better = 1;

        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];
            }
            of = nf;
        }
        done = !(change && better);
    } while (!done && maxTry--);
#if 1
    if (bcc)
        if (((epo_code[0][0] ^ epo_code[0][1]) & 1) != 0 || ((epo_code[0][1] ^ epo_code[0][2]) & 1) != 0 ||

            ((epo_code[1][0] ^ epo_code[1][1]) & 1) != 0 || ((epo_code[1][1] ^ epo_code[1][2]) & 1) != 0)
            printf(
                "bcc parity error %d  %d  %d  %d  %d  %d \n", epo_code[0][0], epo_code[0][1], epo_code[0][2], epo_code[1][0], epo_code[1][1], epo_code[1][2]);

#endif
    return totalError(data, out, numEntries);
};

double ep_shaker_2__(double data[MAX_ENTRIES][DIMENSION], int numEntries, int index_[MAX_ENTRIES], double out[MAX_ENTRIES][DIMENSION], int ns)
{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
#define SIZE 6
    int    i, j, k;
    double mean[DIMENSION];
    int    index[MAX_ENTRIES];

    int maxTry = 8;

    int Mi = 0;

    int Mi_ = (ns == 1 ? 15 : (ns == 2 ? 7 : 7));  // this is last cluster, not max number 0f clusters
    int bits;

    bits = (ns == 1 ? 0 : (ns == 2 ? 2 : 4));

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

    for (k = 0; k < numEntries; k++)
        if (index[k] > Mi_)
        {
            printf("index error\n");
            for (i = 0; i < numEntries; i++)
                index[i] = 0;
            // work-around for constant block quantizer bug
            break;
        }

    index_collapse(index, numEntries);

    int done;
    int change;
    int start = 1;
    int better;

    double of = 0.0;
    double epo[2][DIMENSION];

    do
    {
        for (j = 0; j < DIMENSION; j++)
        {
            for (mean[j] = k = 0; k < numEntries; k++)
                mean[j] += data[k][j];
            mean[j] /= (double)numEntries;
        }

        for (k = 0; k < numEntries; k++)
            Mi = index[k] > Mi ? index[k] : Mi;

        // shift/scale

        int p, q;

        int p0 = -1, q0 = -1;

        double ee = MAX_ENTRIES * 256. * 256. * DIMENSION * 8.;  // 8 = slack

        if (Mi == 0)
            for (j = 0; j < DIMENSION; j++)
            {
                epo[0][j] = floor(mean[j] / ((double)(1 << bits))) * (1 << bits);
                epo[1][j] = floor(mean[j] / ((double)(1 << bits))) * (1 << bits);
                epo[0][j] = epo[0][j] < 0 ? 0 : epo[0][j];
                epo[0][j] = epo[0][j] > 255 ? 255 : epo[0][j];
                epo[1][j] = epo[1][j] < 0 ? 0 : epo[1][j];
                epo[1][j] = epo[1][j] > 255 ? 255 : epo[1][j];
            }

        for (q = 1; Mi != 0 && q * Mi <= Mi_; q++)
            for (p = q * Mi; p <= Mi_; p++)
            {
                //index * q + p-Mi*q

                int cidx[MAX_ENTRIES];

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

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

                // get ideal clustr centers
                double cc[MAX_CLUSTERS_BIG][DIMENSION];
                double cnt[MAX_CLUSTERS_BIG];

                for (j = 0; j < DIMENSION; j++)
                {
                    for (i = 0; i <= Mi_; i++)
                        cnt[i] = cc[i][j] = 0;
                    rp[0][j] = rp[1][j] = 0;
                }

                for (k = 0; k < numEntries; k++)
                {
                    cnt[cidx[k]] += 1;
                    for (j = 0; j < DIMENSION; j++)
                        cc[cidx[k]][j] += data[k][j];
                }

                for (i = 0; i <= Mi_; i++)
                    for (j = 0; j < DIMENSION; j++)
                        if (cnt[i] != 0)
                            cc[i][j] = floor(cc[i][j] / cnt[i] + 0.5);  // more or less ideal location
                        else
                            cc[i][j] = 0;

                // for cluster centers

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

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

                double dd = im[0][0] * im[1][1] - im[0][1] * im[0][1];
                double epd[2][SIZE][DIMENSION];

                int range[2][2][DIMENSION];
                for (i = 0; i < 2; i++)
                    for (j = 0; j < DIMENSION; j++)
                    {
                        range[i][0][j] = 0;
                        range[i][1][j] = SIZE;
                    }

                if (dd == 0)
                {
                    //############## degenerate, usually the same index
                    for (i = 0; i <= Mi_; i++)
                        if (cnt[i] != 0)
                            break;
                    // Make it into ramp
                    for (j = 0; j < DIMENSION; j++)
                    {
                        epa[0][j] = cc[i][j];
                        epa[1][j] = cc[i][j];

                        for (i = 0; i < SIZE; i++)
                        {
                            epd[0][i][j] = floor(epa[0][j] / ((double)(1 << bits))) * (1 << bits) + (i - (SIZE / 2 - 1)) * (1 << bits);
                            epd[1][i][j] = floor(epa[1][j] / ((double)(1 << bits))) * (1 << bits) + (i - (SIZE / 2 - 1)) * (1 << bits);
                            epd[0][i][j] = epd[0][i][j] < 0 ? 0 : epd[0][i][j];
                            epd[0][i][j] = epd[0][i][j] > 255 ? 255 : epd[0][i][j];
                            epd[1][i][j] = epd[1][i][j] < 0 ? 0 : epd[1][i][j];
                            epd[1][i][j] = epd[1][i][j] > 255 ? 255 : epd[1][i][j];
                        }
                        {
                            for (i = 1; i < SIZE; i++)
                            {
                                if (epd[0][i - 1][j] == epd[0][i][j])
                                    range[0][0][j] = i;
                                if (epd[1][i - 1][j] == epd[1][i][j])
                                    range[1][0][j] = i;
                            }
                            for (i = SIZE - 2; i >= 0; i--)
                            {
                                if (epd[0][i + 1][j] == epd[0][i][j])
                                    range[0][0][j] = i + 1;
                                if (epd[1][i + 1][j] == epd[1][i][j])
                                    range[1][0][j] = i + 1;
                            }
                        }
                    }
#if 1
                    for (k = 0; k < numEntries; k++)
                    {
                        index_[k] = index[k] = 0;
                        for (j = 0; j < DIMENSION; j++)
                            out[k][j] = floor(mean[j] / ((double)(1 << bits))) * (1 << bits) + (i - 1) * (1 << bits);
                    }
#endif
                }
                else
                {
                    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[1][0] * rp[0][j] + im[1][1] * rp[1][j]) * Mi_;
                        epa[1][j] = (im[0][0] * rp[0][j] + im[0][1] * rp[1][j]) * Mi_;

                        for (i = 0; i < SIZE; i++)
                        {
                            epd[0][i][j] = floor(epa[0][j] / ((double)(1 << bits))) * (1 << bits) + (i - (SIZE / 2 - 1)) * (1 << bits);
                            epd[1][i][j] = floor(epa[1][j] / ((double)(1 << bits))) * (1 << bits) + (i - (SIZE / 2 - 1)) * (1 << bits);

                            epd[0][i][j] = epd[0][i][j] < 0 ? 0 : epd[0][i][j];
                            epd[0][i][j] = epd[0][i][j] > 255 ? 255 : epd[0][i][j];
                            epd[1][i][j] = epd[1][i][j] < 0 ? 0 : epd[1][i][j];
                            epd[1][i][j] = epd[1][i][j] > 255 ? 255 : epd[1][i][j];
                        }
                        {
                            for (i = 1; i < SIZE; i++)
                            {
                                if (epd[0][i - 1][j] == epd[0][i][j])
                                    range[0][0][j] = i;
                                if (epd[1][i - 1][j] == epd[1][i][j])
                                    range[1][0][j] = i;
                            }
                            for (i = SIZE - 2; i >= 0; i--)
                            {
                                if (epd[0][i + 1][j] == epd[0][i][j])
                                    range[0][0][j] = i + 1;
                                if (epd[1][i + 1][j] == epd[1][i][j])
                                    range[1][0][j] = i + 1;
                            }
                        }
                    }
                }
                int    best_ep[2][DIMENSION];
                double e[DIMENSION];
                int    l0, l1;

                for (j = 0; j < DIMENSION; j++)
                {
                    e[j] = MAX_ENTRIES * 256 * 256 * 4.;
                    for (l0 = range[0][0][j]; l0 < range[0][1][j]; l0++)
                        for (l1 = range[1][0][j]; l1 < range[1][1][j]; l1++)
                        {
                            // ran on points or clusters ??
                            double cv[MAX_CLUSTERS_BIG];
                            double t = 0;
                            int    i1;
                            for (i1 = 0; i1 <= Mi_; i1++)
                            {
                                cv[i1] = floor((epd[1][l1][j] * i1 + epd[0][l0][j] * (Mi_ - i1)) / ((double)Mi_) + 0.5);
                                cv[i1] = cv[i1] < 0 ? 0 : (cv[i1] > 255 ? 255 : cv[i1]);
                            }

                            for (i1 = 0; i1 < numEntries; i1++)
                                t += (cv[cidx[i1]] - data[i1][j]) * (cv[cidx[i1]] - data[i1][j]);
                            if (t < e[j])
                            {
                                e[j]          = t;
                                best_ep[0][j] = l0;
                                best_ep[1][j] = l1;
                            }
                        }
                }
                {
                    double t = 0;
                    for (j = 0; j < DIMENSION; j++)
                        t += e[j];
                    if (t < ee)
                    {
                        ee = t;
                        p0 = p;
                        q0 = q;
                        for (j = 0; j < DIMENSION; j++)
                        {
                            epo[0][j] = epd[0][best_ep[0][j]][j];
                            epo[1][j] = epd[1][best_ep[1][j]][j];
                        }
                    }
                }
            }
        // reconstruct new ep

        // requantize
        // #############

        double pp[64][DIMENSION];
        double c[MAX_ENTRIES][64][DIMENSION];

        for (i = 0; i <= Mi_; i++)
            for (j = 0; j < DIMENSION; j++)
            {
                pp[i][j] = floor(epo[0][j] + (epo[1][j] - epo[0][j]) * (i / (double)Mi_) + 0.5);
                pp[i][j] = pp[i][j] > 0 ? pp[i][j] : 0;
                pp[i][j] = pp[i][j] < 255. ? pp[i][j] : 255.;
            }

        for (k = 0; k < numEntries; k++)
            for (i = 0; i <= Mi_; i++)
                for (j = 0; j < DIMENSION; j++)
                {
                    double t   = (data[k][j] - pp[i][j]);
                    c[k][i][j] = t * t;
                }

        double outg[MAX_ENTRIES][DIMENSION];
        int    idg[MAX_ENTRIES];

        double mag = 0;
        double ma  = 0;

        for (k = 0; k < numEntries; k++)
        {
            idg[k]   = 0;
            double t = c[k][0][0] + c[k][0][1] + c[k][0][2];
            for (i = 1; i <= Mi_; i++)
            {
                double s = c[k][i][0] + c[k][i][1] + c[k][i][2];
                if (s < t)
                {
                    t      = s;
                    idg[k] = i;
                }
            }
            ma += t;
        }
        mag = ma;
        for (k = 0; k < numEntries; k++)
        {
            for (j = 0; j < DIMENSION; j++)
                outg[k][j] = pp[idg[k]][j];
        }

        double nf = totalError(data, outg, numEntries);

        change = 0;
        for (k = 0; k < numEntries; k++)
        {
            change = change || (index[k] * q0 + p0 - Mi * q0 != idg[k]);
        }
        if (!start)
        {
            better = nf < of;
        }
        else
            better = 1;

        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];
            }
            of = nf;
        }
        done = !(change && better);
    } while (!done && maxTry--);
    return totalError(data, out, numEntries);
}

//################################
double reconstruct_rnd__(double  data[MAX_ENTRIES][DIMENSION],
                         int     numEntries,
                         int     index_[MAX_ENTRIES],
                         double  out[MAX_ENTRIES][DIMENSION],
                         int     ns,
                         double  direction[DIMENSION],
                         double* step)
{
#ifdef USE_DBGTRACE
    DbgTrace(());
#endif
#define NP1 31

    static int code1[NP1][3] =  // step, mean div,diamond
        /*
        GOOD
        */
        {{7, 1, 2},   {11, 1, 2},  {15, 1, 2},  {19, 1, 4},  {24, 1, 4},  {29, 1, 4},  {34, 1, 4},  {40, 1, 4},  {46, 1, 4},  {53, 1, 4},  {60, 2, 1},
         {67, 2, 1},  {75, 2, 1},  {83, 2, 1},  {92, 2, 2},  {101, 2, 2}, {111, 2, 2}, {122, 2, 2}, {133, 2, 2}, {145, 2, 2}, {158, 2, 2}, {172, 2, 2},
         {187, 2, 2}, {204, 2, 4}, {223, 2, 4}, {245, 2, 4}, {270, 2, 4}, {301, 4, 1}, {339, 4, 1}, {392, 4, 1}, {448, 4, 1}};

#undef NP2
#define NP2 3

    static int code2[NP2][3] =  // step, mean div,diamond
                                //#######################

        {{7, 4, 2}, {12, 4, 2}, {24, 4, 2}};

    int dir_limit1 = 13;
    int dir_limit2 = 6;

    static int report[3] = {0, 0, 0};

    int(*code)[3];
    int dir_limit = dir_limit1;
    int np;
    //#######################
    if (ns == 3)
    {
        //#######################
        code      = code2;
        dir_limit = dir_limit2;
        np        = NP2;
    }
    else
    {
        code      = code1;
        dir_limit = dir_limit1;
        np        = NP1;
    }

    if (!report[ns])
    {
        report[ns] = 1;

        int bd = 1;
        int s, i;

        printf("Quantizer report, sets %d\n", ns);

        for (i = 0; i < np; i++)
        {
            int td = code[i][1] * code[i][1] * code[i][1] * code[i][2];
            if (td < bd)
                printf("non-monotonic diver codes?? was  %d  now  %d  at %d\n", bd, td, i);
            bd = td;
        }
        printf("Max divider %d \n", bd);
        for (s = i = 0; i < np; i++)
            s += bd / (code[i][1] * code[i][1] * code[i][1] * code[i][2]);
        printf("Soft estimate; position capacity (scaled by max divider) %d, direction %d \n", s, 24 * dir_limit * dir_limit + 2);

        for (s = i = 0; i < np; i++)
            s += bd / (code[i][1] * code[i][1] * code[i][1] * code[i][2]) *
                 (dir_limit < code[i][0] ? 24 * dir_limit * dir_limit + 2 : 24 * code[i][0] * code[i][0] + 2);

        printf("Tight estimate, all+divider %d, bits %f;\n sets left total %d, full sets left assuming full directions %d\n",
               s,
               log((double)s) / log(2.),
               (int)floor(pow(2., ceil(log((double)s) / log(2.))) + 0.5) - s,
               ((int)floor(pow(2., ceil(log((double)s) / log(2.))) + 0.5) - s) / (24 * dir_limit * dir_limit + 2)

        );
        double b = log((double)s) / log(2.) + 24;

        while (bd > 1)
        {
            b--;
            bd /= 2;
        }

        printf("Tight estimate, TOTAL  %g\n", b);
    }

    int           i, j, k;
    static double mean[DIMENSION];

    int ok;
    int maxTry = 10;

    static int index[MAX_ENTRIES];
    static int index_r[MAX_ENTRIES];

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

    double err = 1512 * 512 * 3 * 16;
    double er1;

    double out1[MAX_ENTRIES][DIMENSION];
    // expand/collapse if needed

    if (ns == 1)  // 3 bit indes
        index_expand(index, numEntries, 16);
    else
        index_expand(index, numEntries, MAX_CLUSTERS);

    PCnt += numEntries;

    do
    {
        double idx_mean;

        mds(data, numEntries, index, mean, &idx_mean, direction, step);

        double s = 0, t = 0, q = 0, q1 = 0;

        for (j = 0; j < DIMENSION; j++)
        {
            for (mean[j] = k = 0; k < numEntries; k++)
                mean[j] += data[k][j];
            mean[j] /= (double)numEntries;
        }

        for (k = 0; k < numEntries; k++)
        {
            s += index[k];
            t += index[k] * index[k];
        }

        for (j = 0; j < DIMENSION; j++)
        {
            direction[j] = 0;
            for (k = 0; k < numEntries; k++)
                direction[j] += (data[k][j] - mean[j]) * index[k];

            q += direction[j] * direction[j];
            q1 = fabs(direction[j]) > q1 ? fabs(direction[j]) : q1;
        }

        s /= (double)numEntries;

        t = t - s * s * (double)numEntries;

        t = (t == 0 ? 0. : 1 / t);

        q = sqrt(q);

        *step = t * q;

        for (j = 0; j < DIMENSION; j++)
        {
            direction[j] /= (q1 == 0 ? 1 : q1);
            if (direction[j] >= 1.0001)
                printf("error\n");
        }

        if (*step != 0)
            *step *= q1 / q;

        double step_li = *step;

        static double dir_r[4][DIMENSION];

        static double dir_step_r[2][4][DIMENSION];  // alternative for dir_r

        static double mean_r[8][DIMENSION];
        static double mean_step_r[2][8][DIMENSION];
        static double step_r[2];

        if (step_li > 200000)
        {  // bypass {
            NShakeCnt += numEntries;
            for (i = 0; i < numEntries; i++)
                for (j = 0; j < DIMENSION; j++)
                    out[i][j] = mean[j] + direction[j] * (*step) * (index[i] - idx_mean);
            return totalError(data, out, numEntries);
        }
#define STEP_FACTOR 7.

        if (ns == 1)
        {
#define STEP_DIV 1.
#define MEAN_DIV 1.
            for (j = 0; j < DIMENSION; j++)
                mean_step_r[0][0][j] = mean_step_r[1][0][j] = mean_r[0][j] = floor(mean[j] / MEAN_DIV) * MEAN_DIV;

            int ii[3];

            // cartesian shake
            for (ii[0] = 0; ii[0] < 2; ii[0]++)
                for (ii[1] = 0; ii[1] < 2; ii[1]++)
                    for (ii[2] = 0; ii[2] < 2; ii[2]++)
                        for (j = 0; j < DIMENSION; j++)
                            mean_step_r[0][ii[0] + ii[1] * 2 + ii[2] * 4][j] = mean_step_r[1][ii[0] + ii[1] * 2 + ii[2] * 4][j] =
                                mean_r[ii[0] + ii[1] * 2 + ii[2] * 4][j]     = mean_r[0][j] + MEAN_DIV * ii[j] > 255 ? 255 : mean_r[0][j] + MEAN_DIV * ii[j];

            step_li = floor((step_li * STEP_FACTOR + .5));

            step_li = step_li > 256 * STEP_FACTOR ? 256 * STEP_FACTOR : step_li;

            int    mc = -1;
            double tt = 2;

            for (i = 0; i < DIMENSION; i++)
                if (fabs(fabs(direction[i]) - 1) < tt)
                {
                    tt = fabs(fabs(direction[i]) - 1);
                    mc = i;
                }
            if (mc < 0 && step_li != 0)
            {
                printf("dir mormalizing error");
            }

            for (i = 0; i < 2; i++)
                for (ii[0] = 0; ii[0] < 2; ii[0]++)
                    for (ii[1] = 0; ii[1] < 2; ii[1]++)
                        for (j = 0; j < DIMENSION; j++)
                            dir_step_r[i][ii[0] + 2 * ii[1]][j] = (j == mc)
                                                                      ? direction[j] * (step_li + i) / STEP_FACTOR
                                                                      : ((floor(direction[j] * (step_li + i))) + (j < mc ? ii[j] : ii[j - 1])) / STEP_FACTOR;
            shake_d_s(data, numEntries, idx_mean, dir_step_r, mean_r, index_r, out1);
        }
        else
        {
            // step quantizer in ramp
            for (i = 0; i < np; i++)
                if (step_li * STEP_FACTOR <= code[i][0])
                    break;
            if (i != 0)
                i--;
            if (i == np - 1)
                i--;

            //"low step"

            // bcc*2 and cartesian; starting points for fcc
            for (j = 0; j < 2; j++)
                setMean(mean, mean_step_r[j], code[i + j][1] * code[i + j][1] * code[i + j][1] * code[i + j][2]);

            step_li = code[i][0];

            double df[2];

            df[0] = code[i][0] < dir_limit ? code[i][0] : dir_limit;
            df[1] = code[i + 1][0] < dir_limit ? code[i + 1][0] : dir_limit;

            int    mc = -1;
            double tt = 2;

            for (j = 0; j < DIMENSION; j++)
                if (fabs(fabs(direction[j]) - 1) < tt)
                {
                    tt = fabs(fabs(direction[j]) - 1);
                    mc = j;
                }
            if (mc < 0 && step_li != 0)
            {
                printf("dir normalizing error");
            }

            int ii[3];
            for (k = 0; k < 2; k++)
                for (ii[0] = 0; ii[0] < 2; ii[0]++)
                    for (ii[1] = 0; ii[1] < 2; ii[1]++)
                        for (j = 0; j < DIMENSION; j++)
                            dir_step_r[k][ii[0] + 2 * ii[1]][j] =
                                (j == mc) ? direction[j] * (double)code[i + k][0] / STEP_FACTOR
                                          : (floor(direction[j] * df[k]) + (j < mc ? ii[j] : ii[j - 1])) / df[k] * (double)code[i + k][0] / STEP_FACTOR;
            shake_d_s_s(data, numEntries, MAX_CLUSTERS, idx_mean, dir_step_r, mean_step_r, index_r, out1);
        }

        er1 = totalError(data, out1, numEntries);

        ok = 1;

        if (er1 < err)
        {
            for (i = 0; i < numEntries; i++)
                ok = ok && (index[i] == index_r[i]);

            for (i = 0; i < numEntries; i++)
                index[i] = index_r[i];

            for (i = 0; i < numEntries; i++)
                for (j = 0; j < DIMENSION; j++)
                    out[i][j] = out1[i][j];
            err = er1;
        }
    } while (!ok && maxTry-- > 0);

    return totalError(data, out, numEntries);
}
