/* wasp - Copyright 1991 by Steven Reiz
 * see COPYING and wasp.c for further info
 * wriffdistr.c, 4/12/90 - 30/1/91, 2/6/91, 23/6/91,
 * 3/7/91 - 10/7/91, 8/12/91, 1/1/92, 7/1/92
 * features: SORT_SLICED, SHAM_BLACK
 */

struct hs_t {
    char dist;
    char pad;
    short color;
};

static char *sourcefile=__FILE__;

#include "wasp.h"
#include "wriff.h"
#define SORT_SLICED
/* #define SHAM_BLACK */

static char regind;
static long squares[]={
	0, 1, 4, 9, 16, 25, 36, 49, 64, 81, 100, 121, 144, 169, 196, 225
};


void
compute_distr(int firstrow, int lastrow)
{
    assert(counts!=NULL1);
    if (icolors<=nregs)
        no_distr();
    else {
        switch (distrmeth) {
        case DISTRMETH_MOSTUSED:
            mostused_distr();
        break;
        case DISTRMETH_WORSTFIRST:
            worstfirst_distr();
        break;
        case DISTRMETH_EHB:
            ehb_distr();
        break;
        case DISTRMETH_MUE:
            mue_distr();
        break;
        case DISTRMETH_HAMSHARP:
            hamsharp_distr();
        break;
        case DISTRMETH_CONTRACTION:
            contraction_distr();
        break;
        default:
	    error0(E0_FATAL, E1_IFF, E2_OPTION, E3_DISTRMETH);
        break;
        }
    }
    if (xmode!=EHB) {
#ifdef SORT_SLICED
        sort_cm();
#else
        if (!slicedo)
            sort_cm();
#endif
    }
    if (slicedo) {
	if (sliced==SLICED_PCHG)
	    restrict_changes=PCHG_NCHANGES;
	if (restrict_changes!= -1)
	    restrict_cm();
    }
    count_pixels(firstrow, lastrow, 1);
    fill_newcol();
}


void
no_distr(void)
{
    short i;
    u_long th, *p;
    u_short *q;

    p=counts+NRGB;
    th=threshold;
    q=curcm;
    i=NRGB-1;
    if (th<=1) {
        do {
            if (*--p)
                *q++ =i;
        } while (--i>=0);
    } else {
        do {
            if (*--p >=th)
                *q++ =i;
        } while (--i>=0);
    }
    assert(q==curcm+icolors);
    while (q<curcm+nregs)
        *q++ =0;
}


void
mostused_distr(void)
{
    static u_long regcounts[MAXNREGS];
    u_long *countp, *countp2, th;
    static u_long *savecountp;
    u_short *cmp;
    short i;
    
    countp=regcounts;
    i=nregs-1;
    do {
        *countp++ =0;
    } while (--i>=0);
    th=0;
    countp=counts+NRGB;
    i=NRGB-1;
    do {
        if (*--countp>th) {
            th= *countp;
            savecountp=countp;
            countp=regcounts;
            do {
                /* tight loop! */
            } while (*countp++ >=th);
            countp2=regcounts+nregs;
            cmp=curcm+nregs;
            if (countp2>countp) {
                do {
                    *--countp2= *(countp2-2);
                    *--cmp    = *(cmp-2);
                } while (countp2>countp);
            }
            *--countp2=th;
            *--cmp=i;
            th=regcounts[nregs-1];
            countp=savecountp;
        }
    } while (--i>=0);
#ifdef SHAM_BLACK
	if (slicedo) {
		for (i=0; i<nregs; ++i)
			if (curcm[i]==0x000)
				break;
		if (i>=nregs) { /* black isn't amongst the colors, add it */
			curcm[nregs-1]=0x000;
		}
	}
#endif
}


void
worstfirst_distr(void)
{
	static int sicolors= -1;
	static int startreg;
	int i;
	short rgb1, rgb2;

    if (sicolors<icolors) {
	if (sicolors== -1) {
            if (icolors<128)
                sicolors=128;
            else
                sicolors=icolors;
        } else {
            free(wfmindist);
            free(wfminind);
            free(wfrgb);
            if (icolors<256)
                sicolors=256;
            else
                sicolors=icolors;
        }
        wfmindist=Malloc(sicolors);
        wfminind=Malloc(sicolors);
        wfrgb=Malloc(sicolors*3);
    }
    fill_wf_rgb();
    startreg=2;
#ifdef SHAM_BLACK
    if (slicedo) {
	startreg=1;
	rgb1=0x000; regind=0;
	fill_wf_dist_1(rgb1);
    } else {
#endif
	find_2_furthest(&rgb1, &rgb2);
	curcm[0]=rgb1; regind=0;
	fill_wf_dist_1(rgb1);
	curcm[1]=rgb2; regind=1;
	fill_wf_dist_2(rgb2);
#ifdef SHAM_BLACK
    }
#endif
    for (i=startreg; i<nregs; ++i) {
        find_1_furthest(&rgb1);
        curcm[i]=rgb1; regind=i;
        fill_wf_dist_2(rgb1);
    }
    fill_wf2rgbweight();
    wf2_redo_curcm();
}


void
fill_wf_rgb(void)
{
    long i;
    u_long th, *p;
    char *q;

    p=counts+NRGB;
    th=threshold;
    q=wfrgb;
    i=NRGB-1;
    if (th<=1) {
        do {
            if (*--p) {
                *q++ =i>>8;
                *q++ =(i>>4)&0x0f;
                *q++ =i&0x0f;
            }
        } while (--i>=0);
    } else {
        do {
            if (*--p >=th) {
                *q++ =i>>8;
                *q++ =(i>>4)&0x0f;
                *q++ =i&0x0f;
            }
        } while (--i>=0);
    }
    assert(q==wfrgb+3*icolors);
}


void
find_2_furthest(short *rgb1p, short *rgb2p)
{
    REG char *p, *q;
    REG char r, g, b, t, dist, bigdist;
    NON_REG char *bigp, *bigq;

    bigdist=0;
    p=wfrgb+3*icolors;
    while (p>wfrgb) {
        b= *--p;
        g= *--p;
        r= *--p;
        q=wfrgb;
        do {
            if ((dist=r- *q++)<0) dist= -dist;
            if ((t=g- *q++)<0)    dist-=t; else dist+=t;
            if ((t=b- *q++)<0)    dist-=t; else dist+=t;
            if (dist>bigdist) {
                bigdist=dist;
                bigp=p;
                bigq=q;
            }
        } while (q<p);
    }
    *rgb1p=(*bigp<<8)    |(*(bigp+1)<<4)|(*(bigp+2));
    *rgb2p=(*(bigq-3)<<8)|(*(bigq-2)<<4)|(*(bigq-1));
}


void
fill_wf_dist_1(short color)
{
    short i;
    char r, g, b, t, dist;
    char *mdp, *mip, *rgbp;

    r=color>>8;
    g=(color>>4)&0x0f;
    b=color&0x0f;
    mdp=wfmindist+icolors;
    mip=wfminind+icolors;
    rgbp=wfrgb+3*icolors;
    i=icolors-1;
    do {
        if ((dist=b- *--rgbp)<0) dist= -dist;
        if ((t=g- *--rgbp)<0)    dist-=t; else dist+=t;
        if ((t=r- *--rgbp)<0)    dist-=t; else dist+=t;
        *--mdp=dist;
        *--mip=regind;
    } while (--i>=0);
}


void
fill_wf_dist_2(short color)
{
    short i;
    char r, g, b, t, dist;
    char *mdp, *mip, *rgbp;

    r=color>>8;
    g=(color>>4)&0x0f;
    b=color&0x0f;
    mdp=wfmindist+icolors;
    mip=wfminind+icolors;
    rgbp=wfrgb+3*icolors;
    i=icolors-1;
    do {
        if ((dist=b- *--rgbp)<0) dist= -dist;
        if ((t=g- *--rgbp)<0)    dist-=t; else dist+=t;
        if ((t=r- *--rgbp)<0)    dist-=t; else dist+=t;
        if (dist< *--mdp) {
            *mdp=dist;
            *--mip=regind;
        } else
            --mip;
    } while (--i>=0);
}


void
find_1_furthest(short *rgbp)
{
    char *p, max;
    short i, maxi;

    p=wfmindist+icolors;
    max=0;
    i=icolors-1;
    do {
        if (*--p>max) {
            max= *p;
            maxi=i;
        }
    } while (--i>=0);
    p=wfrgb+3*maxi;
    *rgbp=(*p<<8)|(*(p+1)<<4)|(*(p+2));
}


void
fill_wf2rgbweight(void)
{
    char *rgbp, *mip;
    u_long *weightp, weight;
    short i;
    char r, g, b;

    if (wf2rgbweight==NULL1)
        wf2rgbweight=Malloc(nregs*4*sizeof(u_long));
    weightp=wf2rgbweight;
    i=nregs-1;
    do {
        *weightp++ =0;
        *weightp++ =0;
        *weightp++ =0;
        *weightp++ =0;
    } while (--i>=0);
    rgbp=wfrgb;
    mip=wfminind;
    i=icolors-1;
    do {
        r= *rgbp++;
        g= *rgbp++;
        b= *rgbp++;
        weight=counts[(r<<8)|(g<<4)|b];
        weightp=wf2rgbweight+4* (*mip++);
        *weightp++ += r*weight;
        *weightp++ += g*weight;
        *weightp++ += b*weight;
        *weightp   += weight;
    } while (--i>=0);
}


void
wf2_redo_curcm(void)
{
    u_long *weightp, weight, r, g, b;
    short i;
    u_short *cmp;
    
    weightp=wf2rgbweight;
    cmp=curcm;
    i=nregs-1;
    do {
        r= *weightp++;
        g= *weightp++;
        b= *weightp++;
        weight= *weightp++;
        if (!weight)
            weight=1;
        r=(r+weight/2)/weight;
        if (r>15) r=15;
        g=(g+weight/2)/weight;
        if (g>15) g=15;
        b=(b+weight/2)/weight;
        if (b>15) b=15;
        *cmp++ =(r<<8)|(g<<4)|b;
    } while (--i>=0);
#ifdef SHAM_BLACK
    if (slicedo)
	black_darkest();
#endif
}


void
black_darkest(void)
{
	short i, besti, val, bestval;

	bestval=100;
	for (i=0; i<nregs; ++i) {
		val=(curcm[i]&0xf)+((curcm[i]>>4)&0xf)+(curcm[i]>>8);
		if (val<bestval) {
			bestval=val;
			besti=i;
		}
	}
	curcm[besti]=0x000;
}


void
ehb_distr(void)
{
    static int sicolors= -1;
    int i, nr;
    short rgb1, rgb2;

    nr=nregs/2;
    if (sicolors<icolors) {
        if (sicolors== -1) {
            if (icolors<128)
                sicolors=128;
            else
                sicolors=icolors;
        } else {
            free(wfmindist);
            free(wfminind);
            free(wfrgb);
            if (icolors<256)
                sicolors=256;
            else
                sicolors=icolors;
        }
        wfmindist=Malloc(sicolors);
        wfminind=Malloc(sicolors);
        wfrgb=Malloc(sicolors*3);
    }
    fill_wf_rgb();
    find_2_furthest(&rgb1, &rgb2);
    regind=0;
    fill_wf_dist_1(rgb1);
    ehb_pair(rgb1, 0);
    for (i=1; i<nr; ++i) {
        find_1_furthest(&rgb1);
        regind=i;
        fill_wf_dist_2(rgb1);
        ehb_pair(rgb1, i);
    }
    fill_wf2rgbweight();
    ehb_redo_curcm();
}


void
ehb_pair(short c1, int i)
{
    short c2, c3, c4;
    short r, g, b;
    int nr;

    nr=nregs/2;
    r=c1>>8;
    g=(c1>>4)&0x0f;
    b=c1&0x0f;
    c2=((r>>1)<<8)|((g>>1)<<4)|(b>>1);
    if (r<8 && g<8 && b<8) {
        c3=(r<<9)|(g<<5)|(b<<1);
        ehb_worst(c2, c3, &c4);
        if (c4==c2) {
            curcm[i]=c1;
            curcm[nr+i]=c2;
            regind=nr+i;
        } else {
            curcm[i]=c4;
            curcm[nr+i]=c1;
            c2=c4;
            ehb_minind_adjust((int)i, (int)(nr+i));
            regind=i;
        }
    } else {
        curcm[i]=c1;
        curcm[nr+i]=c2;
        regind=nr+i;
    }
    fill_wf_dist_2(c2);
}


void
ehb_worst(short col1, short col8, short *worstp)
{
    short curcol;
    int curdist, bestdist;
    short i;

    bestdist=ehb_farcol(col1);
    *worstp=col1;
    i=7;
    do {
        curcol=col8;
        if (i&1)
            curcol|=0x001;
        if (i&2)
            curcol|=0x010;
        if (i&4)
            curcol|=0x100;
        curdist=ehb_farcol(curcol);
        if (curdist>bestdist) {
            bestdist=curdist;
            *worstp=curcol;
        }
    } while (--i>=0);
}


int
ehb_farcol(short color)
{
    char r, g, b;
    char *p, *q;
    char t, dist, bigdist;
    short i;

    r=color>>8;
    g=(color>>4)&0x0f;
    b=color&0x0f;
    bigdist=0;
    i=icolors-1;
    p=wfmindist;
    q=wfrgb;
    do {
        if ((dist=r- *q++)>0) dist= -dist;
        if ((t=g- *q++)>0)    dist-=t; else dist+=t;
        if ((t=b- *q++)>0)    dist-=t; else dist+=t;
        dist+= *p++;
        if (dist>bigdist)
            bigdist=dist;
    } while (--i>=0);
    return (int)bigdist;
}


void
ehb_minind_adjust(int from, int to)
{
    char *p;
    char fromc, toc;
    short i;

    p=wfminind+icolors;
    i=icolors-1;
    fromc=from;
    toc=to;
    do {
        if (*--p ==fromc)
            *p=toc;
    } while (--i>=0);
}


void
ehb_redo_curcm(void)
{
    short nr, i;
    long r1, g1, b1, w1, r2, g2, b2, w2;
    u_long *p1, *p2;

    nr=nregs/2;
    p1=wf2rgbweight;
    p2=wf2rgbweight+4*nr;
    for (i=0; i<nr; ++i) {
        r1= *p1++;     g1= *p1++;     b1= *p1++;     w1= *p1++;
        r2=(*p2++)<<1; g2=(*p2++)<<1; b2=(*p2++)<<1; w2= *p2++;
        w1=w1+w2;
        assert(w1);
        w2=w1/2;
        r1=(r1+r2+w2)/w1;
        if (r1>15) r1=15;
        g1=(g1+g2+w2)/w1;
        if (g1>15) g1=15;
        b1=(b1+b2+w2)/w1;
        if (b1>15) b1=15;
        curcm[i]=(r1<<8)|(g1<<4)|b1;
        r1>>=1; g1>>=1; b1>>=1;
        curcm[nr+i]=(r1<<8)|(g1<<4)|b1;
    }
}


void
mue_distr(void)
{
    static u_long regcounts[MAXNREGS];
    u_long *countp, *countp2, th;
    static u_long *savecountp;
    u_short *cmp;
    short i, nr;
    
    nr=nregs/2;
    countp=regcounts;
    i=nr-1;
    do {
        *countp++ =0;
    } while (--i>=0);
    th=0;
    countp=counts+NRGB;
    i=NRGB-1;
    do {
        if (*--countp>th) {
            th= *countp;
            savecountp=countp;
            countp=regcounts;
            do {
                /* tight loop! */
            } while (*countp++ >=th);
            countp2=regcounts+nr;
            cmp=curcm+nr;
            if (countp2>countp) {
                do {
                    *--countp2= *(countp2-2);
                    *--cmp    = *(cmp-2);
                } while (countp2>countp);
            }
            *--countp2=th;
            *--cmp=i;
            th=regcounts[nr-1];
            countp=savecountp;
        }
    } while (--i>=0);
    mue_extend();
}


void
mue_extend(void)
{
    short i;
    short r, g, b, color;
    u_short *p, *q;

    i=nregs/2;
    p=curcm;
    q=p+i;
    --i;
    do {
        color= *p++;
        r=color>>9;
        g=(color>>5)&0x07;
        b=(color>>1)&0x07;
        *q++ = (r<<8)|(g<<4)|b;
    } while (--i>=0);
}


void
hamsharp_distr(void)
{
    static int sicolors= -1;
    int i;

    if (sicolors<icolors) {
        if (sicolors!= -1) {
            for (i=0; i<sicolors; ++i)
                free(hshead[i]);
            free(hshead);
            free(hsmark);
            free(wfminind);
            free(wfrgb);
        }
        sicolors=icolors;
        wfrgb=Malloc(sicolors*3);
        wfminind=Malloc(sicolors);
        hsmark=Malloc(sicolors);
        hshead=Malloc(sicolors*sizeof(void *));
        for (i=0; i<sicolors; ++i)
            hshead[i]=Malloc(sicolors*sizeof(struct hs_t));
    }
    fill_wf_rgb();
    fill_hsmark();
    fill_hshead();
    fill_hs_cm();
    fill_wf2rgbweight();
    wf2_redo_curcm();
}


void
fill_hsmark(void)
{
    short i;
    char *q;

    q=hsmark;
    i=icolors-1;
    do {
        *q++ =1;
    } while (--i>=0);
}


int
cmphs(struct hs_t *p1, struct hs_t *p2)
{
    return p1->dist - p2->dist;
}


void
fill_hshead(void)
{
    REG char *p, *q;
    REG struct hs_t *hp;
    NON_REG int ic;
    REG short i;
    REG char r, g, b, t, dist;

    p=wfrgb+3*icolors;
    ic=icolors;
    while (p>wfrgb) {
        b= *--p;
        g= *--p;
        r= *--p;
        q=wfrgb+3*icolors;
        hp=hshead[--ic];
        i=icolors-1;
        do {
            if ((dist=b- *--q)<0) dist= -dist;
            if ((t=g- *--q)<0)    dist-=t; else dist+=t;
            if ((t=r- *--q)<0)    dist-=t; else dist+=t;
            hp->dist=dist;
            hp->color=i;
            ++hp;
        } while (--i>=0);
        qsort((char *)hshead[ic], (int)icolors, sizeof(struct hs_t),
	 (int (*)(void *, void *))cmphs);
    }
}


void
fill_hs_cm(void)
{
    NON_REG short j;
    REG short i, k, bestk, colperreg;
    REG long dist, bestdist;
    REG struct hs_t *p;
    REG char *q, *r;

    colperreg=icolors/nregs;
    q=hsmark;
    for (j=0; j<nregs; ++j) {
        if (j==nregs-1)
            colperreg+=icolors-nregs*colperreg;
        bestdist=1000000000L;
        for (k=0; k<icolors; ++k) {
            dist=0;
            p=hshead[k];
            i=colperreg-1;
            do {
                while (!q[p->color])
                    ++p;
                dist+=p->dist;
                ++p;
            } while (--i>=0);
            if (dist<bestdist) {
                bestdist=dist;
                bestk=k;
            }
        }
        p=hshead[bestk];
        i=colperreg-1;
        do {
            while (!q[p->color])
                ++p;
            q[p->color]=0;
            wfminind[p->color]=j;
            ++p;
        } while (--i>=0);
        r=wfrgb+3*bestk;
        curcm[j]=(*r<<8)|(*(r+1)<<4)|*(r+2);
    }
}


#define CONTRACT2

void
contraction_distr(void)
{
    static int sicolors= -1;
    int i;

    if (sicolors!= -1) {
        for (i=0; i<sicolors; ++i)
            if (cthead[i])
                free(cthead[i]);
        free(cthead);
        free(ctrgbw);
    }
    sicolors=icolors;
    ctrgbw=Malloc(sicolors*4*sizeof(float));
    cthead=Malloc(sicolors*sizeof(void *));
    cthead[0]=NULL;
    for (i=1; i<sicolors; ++i)
        cthead[i]=Malloc(i*sizeof(float));
    fill_ctrgbw();
    fill_cthead();
    do_contraction();
    fill_ct_cm();
}


void
fill_ctrgbw(void)
{
    short i;
    u_long th, *p;
    float *q;

    p=counts+NRGB;
    th=threshold;
    q=ctrgbw;
    i=NRGB-1;
    do {
        if (*--p >=th) {
            *q++ =(float)(i>>8);
            *q++ =(float)((i>>4)&0x0f);
            *q++ =(float)(i&0x0f);
            *q++ =(float)*p;
        }
    } while (--i>=0);
    assert(q==ctrgbw+4*icolors);
}


void
fill_cthead(void)
{
    short i, j;
    float *p, *q;
    float r, g, b, dist, t;

    for (i=1; i<icolors; ++i) {
        p=cthead[i];
        q=ctrgbw+4*i;
        r= *q++;
        g= *q++;
        b= *q;
        q=ctrgbw;
#ifdef CONTRACT2
        for (j=0; j<i; ++j) {
            t=r- *q++; dist=t*t;
            t=g- *q++; dist+=t*t;
            t=b- *q++; dist+=t*t;
            ++q;
            *p++ =dist;
        }
#else
        for (j=0; j<i; ++j) {
            if ((dist=r- *q++)<0) dist= -dist;
            if ((t=g- *q++)<0)    dist-=t; else dist+=t;
            if ((t=b- *q++)<0)    dist-=t; else dist+=t;
            ++q;
            *p++ =dist;
        }
#endif
    }
}


void
do_contraction(void)
{
    short ncolors, i, j, besti, bestj;
    float *p, *q, bestdist;
    float newr, newg, newb, neww, wi, wj;
    float t, dist;

    ncolors=icolors;
    while (ncolors>nregs) {
        /* find minimum pair */
        bestdist=2000.0;
        for (i=0; i<icolors; ++i) {
            p=cthead[i];
            if (!p)
                continue;
            j=0;
            do {
                if (*p++ <bestdist) {
                    besti=i;
                    bestj=j;
                    bestdist= *(p-1);
                }
            } while (++j<i);
        }
        /* contract pair to new pair */
        p=ctrgbw+4*besti;
        q=ctrgbw+4*bestj;
        wi=p[3];
        wj=q[3];
        neww=wi+wj;
        newr=(wi*p[0]+wj*q[0])/neww;
        newg=(wi*p[1]+wj*q[1])/neww;
        newb=(wi*p[2]+wj*q[2])/neww;
        /* delete 1 of pair */
        if (bestj>besti) {
            i=besti;
            besti=bestj;
            bestj=i;
        }
        free(cthead[besti]);
        cthead[besti]=NULL;
        ctrgbw[4*besti+3]= -1.0;
        for (i=besti+1; i<icolors; ++i) {
            p=cthead[i];
            if (p)
                p[besti]=1000.0;
        }
        /* replace other of pair by new contracted color */
        p=ctrgbw+4*bestj;
        *p++ =newr;
        *p++ =newg;
        *p++ =newb;
        *p   =neww;
        for (i=bestj+1, q=ctrgbw+4*i; i<icolors; ++i) {
            p=cthead[i];
            if (p) {
#ifdef CONTRACT2
                t=newr- *q++; dist=t*t;
                t=newg- *q++; dist+=t*t;
                t=newb- *q++; dist+=t*t;
#else
                if ((dist=newr- *q++)<0) dist= -dist;
                if ((t=newg- *q++)<0)    dist-=t; else dist+=t;
                if ((t=newb- *q++)<0)    dist-=t; else dist+=t;
#endif
                ++q;
                p[bestj]=dist;
            } else {
                q+=4;
            }
        }
        for (i=0, p=cthead[bestj]; i<bestj; ++i) {
            q=ctrgbw+4*i;
            if (q[3]< -0.5) {
                *p++ =1000.0;
            } else {
#ifdef CONTRACT2
                t=newr- *q++; dist=t*t;
                t=newg- *q++; dist+=t*t;
                t=newb- *q;   dist+=t*t;
#else
                if ((dist=newr- *q++)<0) dist= -dist;
                if ((t=newg- *q++)<0)    dist-=t; else dist+=t;
                if ((t=newb- *q)<0)      dist-=t; else dist+=t;
#endif
                *p++ =dist;
            }
        }
        /* decrease number of colors */
        --ncolors;
    }
}


void
fill_ct_cm(void)
{
    float *p;
    u_short *q, color;
    short i;

    p=ctrgbw;
    q=curcm;
    i=icolors-1;
    do {
        if (p[3]< -0.5) {
            p+=4;
        } else {
            color =((int)(*p++ + 0.5)<<8)&0xf00;
            color|=((int)(*p++ + 0.5)<<4)&0x0f0;
            color|=((int)(*p++ + 0.5))   &0x00f;
            ++p;
            *q++ =color;
        }
    } while (--i>=0);
    assert(q==curcm+nregs);
#ifdef SHAM_BLACK
    if (slicedo)
	black_darkest();
#endif
}


void
fill_cmrgb(void)
{
    short i, color;
    u_short *p;
    char *q;

    if (cmrgb==NULL1)
	cmrgb=Malloc(3*nregs);
    p=curcm;
    q=cmrgb;
    i=nregs-1;
    do {
        color= *p++;
        *q++ = color>>8;
        *q++ = (color>>4)&0x0f;
        *q++ = color&0x0f;
    } while (--i>=0);
}


void
fill_newcol(void)
{
    short i, mini;
    char r, g, b, min, dist, t;
    NON_REG short savei;
    u_long *countp;
    char *rgbp;

    fill_cmrgb();
    if (newcol==NULL1) {
	newcol=Malloc(NRGB);
	error=Malloc(NRGB);
	error2=Malloc(NRGB*sizeof(u_long));
    }
    i=NRGB-1;
    countp=counts+NRGB;
    do {
        if (*--countp) {
            savei=i;
            r=i>>8;
            g=(i>>4)&0x0f;
            b=i&0x0f;
            min=100;
            rgbp=cmrgb+3*nregs;
            i=nregs-1;
            do {
                if ((dist=b- *--rgbp)<0) dist= -dist;
                if ((t=g- *--rgbp)<0)    dist-=t; else dist+=t;
                if ((t=r- *--rgbp)<0)    dist-=t; else dist+=t;
                if (dist<min) {
                    min=dist;
                    mini=i;
                }
            } while (--i>=0);
            i=savei;
            newcol[i]=mini;
            error[i]=min;
	    rgbp=cmrgb+3*mini;
	    if ((t=r- *rgbp++)<0) t= -t; error2[i]=squares[t];
	    if ((t=g- *rgbp++)<0) t= -t; error2[i]+=squares[t];
	    if ((t=b- *rgbp  )<0) t= -t; error2[i]+=squares[t];
        }
    } while (--i>=0);
}


void
fill_newcol_count(void)
{
    short i, mini;
    char r, g, b, min, dist, t;
    NON_REG short savei;
    u_long *countp;
    char *rgbp;

    fill_cmrgb();
    if (newcol==NULL1) {
	newcol=Malloc(NRGB);
	error=Malloc(NRGB);
	error2=Malloc(NRGB*sizeof(u_long));
    }
    i=NRGB-1;
    countp=counts+NRGB;
    do {
        if (*--countp) {
            savei=i;
            r=i>>8;
            g=(i>>4)&0x0f;
            b=i&0x0f;
            min=100;
            rgbp=cmrgb+3*nregs;
            i=nregs-1;
            do {
                if ((dist=b- *--rgbp)<0) dist= -dist;
                if ((t=g- *--rgbp)<0)    dist-=t; else dist+=t;
                if ((t=r- *--rgbp)<0)    dist-=t; else dist+=t;
                if (dist<min) {
                    min=dist;
                    mini=i;
                }
            } while (--i>=0);
            i=savei;
            newcol[i]=mini;
            error[i]=min;
        }
    } while (--i>=0);
}


void
restrict_cm()
{
	/* cm: array with all the colormaps, each nregs u_shorts
	 * curcm: pointer into cm, pointing to the unrestricted
	 *  colormap for the current line (or two lines in LACE)
	 * curcm-nregs: pointer into cm, pointing to the colormap
	 *  for the previous line
	 * newmap: the restricted map for the current line that is
	 *  produced by this function, curcm is overwritten with newmap
	 * restrict_changes: max. nr. of registers may be different in
	 *  curcm-nregs and the restricted curcm
	 * reg_dist: reg_dist[oldn][newn] is the (manhattan) distance
	 *  between (curcm-nregs)[oldn] and curcm[newn]
	 * oldmin: oldmin[oldn] contains the minimum of reg_dist[oldn][x]
	 * newmin: newmin[newn] contains the minimum of reg_dist[x][newn]
	 * newrgb: newrgb[newn] contains r,g,b of curcm[newn]
	 */
	u_short newmap[MAXNREGS];
	u_short *oldp, *newp, color;
	short i, j, ntodo, nregs1;
	struct reg_min {
		char reg;
		char dist;
	} oldmin[MAXNREGS], newmin[MAXNREGS], *minp;
	struct reg_rgb {
		char r, g, b;
	} newrgb[MAXNREGS], *rgbp;
	char *reg_dist[MAXNREGS], *charp;

	if (curcm==cm) {	/* the first line is handled by CMAP */
		*curcm=0;	/* make sure(!) register 0 is black */
		return;
	}
	nregs1=nregs-1;
	/* mark all registers in newmap unoccupied */
	i=nregs1;
	newp=newmap;
	do {
		*newp++ =0xffff;
	} while (--i>=0);
	/* give register 0 the value black */
	oldp=curcm-nregs;
	assert(*oldp==0);
	*oldp|=0x8000;
	newmap[0]=0;
	/* handle the colors in curcm that were already present in curcm-nregs */
	newp=curcm;
	ntodo=nregs;
	i=nregs1;
	do {
		color= *newp;
		oldp=curcm-nregs;
		j=nregs1;
		do {
			if ((*oldp & 0x0fff) == color) {
				*oldp|=0x8000;
				*newp|=0x8000;
				newmap[nregs1-j]=color;
				--ntodo;
				break;
			}
			++oldp;
		} while (--j>=0);
		++newp;
	} while (--i>=0);
	/* handle at most restrict_changes in curcm that were not present in
	 * curcm-nregs by searching min(ntodo, restrict_changes) times a pair
	 * of regs oldreg,newreg in curcm-nregs and curcm such that oldreg
	 * is not marked keep yet, newreg is not marked done yet, oldreg
	 * has the largest minumum distance to the regs not done yet in curcm
	 * (so we will overwrite the most useless register first),
	 * newreg has the largest minimum distance to the regs marked keep
	 * in curcm-nregs (so we will do the most interesting register first).
	 */
	/* compute min(ntodo, restrict_changes) */
	if (restrict_changes<ntodo) {
		nrestricted+=ntodo-restrict_changes;
		ntodo=restrict_changes;
	}
	/* compute min(ntodo, #unoccupied regs in newmap) */
	i=nregs1;
	j=0;
	newp=newmap;
	do {
		if (*newp++ ==0xffff)
			++j;
	} while (--i>=0);
	if (j<ntodo) {
		nrestricted+=ntodo-j;
		ntodo=j;
	}
	/* initialization of distance and min.distance tables */
	reg_dist[0]=NULL;
	if (ntodo) {
		/* allocate reg_dist */
		for (i=0; i<nregs; ++i)
			reg_dist[i]=Malloc(nregs);
		/* fill newrgb[newn] with the r,g,b values */
		newp=curcm;
		rgbp=newrgb;
		i=nregs1;
		do {
			color= *newp++;
			if (color & 0x8000)
				rgbp->g= -1;
			else {
				rgbp->r=color>>8;
				rgbp->g=(color>>4)&0x0f;
				rgbp->b=color&0x0f;
			}
			++rgbp;
		} while (--i>=0);
		/* compute reg_dist[oldn][newn] */
		for (i=0; i<nregs; ++i) {
			char r, g, b, dist, t;

			color=(curcm-nregs)[i]&0x0fff;
			r=color>>8;
			g=(color>>4)&0x0f;
			b=color&0x0f;
			charp=reg_dist[i];
			rgbp=newrgb;
			j=nregs1;
			do {
				if (rgbp->g == -1)
					*charp++ =100;	/* not used */
				else {
					if ((dist=r-rgbp->r)<0) dist= -dist;
					if ((t=g-rgbp->g)<0)	dist-=t; else dist+=t;
					if ((t=b-rgbp->b)<0)	dist-=t; else dist+=t;
					*charp++ =dist;
				}
				++rgbp;
			} while (--j>=0);
		}
		/* fill oldmin[oldn] with minimum of reg_dist[oldn][x] */
		minp=oldmin;
		for (i=0; i<nregs; ++i) {
			if (newmap[i]==0xffff) {
			/* implies that (curcm-nregs)[i] isn't marked keep */
				char minval=100;
				int mini;

				charp=reg_dist[i];
				j=nregs1;
				do {
					if (*charp<minval) {
						minval= *charp;
						mini=nregs1-j;
					}
					++charp;
				} while (--j>=0);
				minp->reg=mini;
				minp->dist=minval;
			} else {
				minp->dist= -1;
			}
			++minp;
		}
		/* fill newmin[newn] with minimum of reg_dist[x][newn] */
		minp=newmin;
		rgbp=newrgb;
		for (i=0; i<nregs; ++i) {
			if (rgbp->g== -1) {
				minp->dist= -1;
			} else {
				char minval=100;
				int mini;

				for (j=0; j<nregs; ++j) {
					if (newmap[j]!=0xffff
					 && reg_dist[j][i]<minval) {
						minval=reg_dist[j][i];
						mini=j;
					}
				}
				minp->reg=mini;
				minp->dist=minval;
			}
			++minp;
			++rgbp;
		}
	}
	/* one by one search for the ntodo register pairs */
	while (ntodo>0) {
		int oldreg= -1, newreg= -1;
		char maxval;

		--ntodo;
		/* find the oldreg */
		maxval= -1;
		minp=oldmin;
		i=nregs1;
		do {
			if (minp->dist>maxval) {
				maxval=minp->dist;
				oldreg=nregs1-i;
			}
			++minp;
		} while (--i>=0);
		assert(oldreg>=0);
		/* find the newreg */
		maxval= -1;
		minp=newmin;
		i=nregs1;
		do {
			if (minp->dist>maxval) {
				maxval=minp->dist;
				newreg=nregs1-i;
			}
			++minp;
		} while (--i>=0);
		assert(newreg>=0);
		/* mark newreg done, oldreg keep, and fill in value */
		color=curcm[newreg];
		assert(!(color & 0xf000));
		assert(newmap[oldreg]==0xffff);
		newmap[oldreg]=color;
		oldmin[oldreg].dist= -1;
		newmin[newreg].dist= -1;
		newrgb[newreg].g= -1;
		/* fix up oldmin */
		minp=oldmin;
		for (i=0; i<nregs; ++i) {
			if (newmap[i]==0xffff) {
				charp=reg_dist[i];
				charp[newreg]=100;
				if (minp->reg==newreg) {
					char minval=100;
					int mini;

					j=nregs1;
					do {
						if (*charp<minval) {
							minval= *charp;
							mini=nregs1-j;
						}
						++charp;
					} while (--j>=0);
					minp->reg=mini;
					minp->dist=minval;
				}
			}
			++minp;
		}
		/* fix up one row of reg_dist */
		{
			char r, g, b, dist, t;

			color=newmap[oldreg];
			r=color>>8;
			g=(color>>4)&0x0f;
			b=color&0x0f;
			charp=reg_dist[oldreg];
			rgbp=newrgb;
			j=nregs1;
			do {
				if (rgbp->g == -1)
					*charp++ =100;	/* not used */
				else {
					if ((dist=r-rgbp->r)<0) dist= -dist;
					if ((t=g-rgbp->g)<0)	dist-=t; else dist+=t;
					if ((t=b-rgbp->b)<0)	dist-=t; else dist+=t;
					*charp++ =dist;
				}
				++rgbp;
			} while (--j>=0);
		}
		/* fix up newmin */
		minp=newmin;
		rgbp=newrgb;
		for (i=0; i<nregs; ++i) {
			if (rgbp->g!= -1) {
				if (reg_dist[oldreg][i]<minp->dist) {
					minp->dist=reg_dist[oldreg][i];
					minp->reg=oldreg;
				}
			}
			++minp;
			++rgbp;
		}
	}
	/* clean up distance table */
	if (reg_dist[0]) {
		for (i=0; i<nregs; ++i)
			free(reg_dist[i]);
	}
	/* the still unoccupied regs in newmap will have to be filled with
	 * the same value as in curcm-nregs, strip hi bits from curcm-nregs
	 */
	i=nregs1;
	newp=newmap;
	oldp=curcm-nregs;
	do {
		*oldp&=0x0fff;
		if (*newp & 0xf000)
			*newp++ = *oldp++;
		else {
			++newp;
			++oldp;
		}
	} while (--i>=0);
	/* overwrite curcm with newmap */
	i=nregs1;
	newp=curcm;
	oldp=newmap;
	do {
		*newp++ = *oldp++;
	} while (--i>=0);
}