//
// my_sqrt16.c - test square rot algorithm
//
// Copyright (C) 2006  Nathan (Acorn) Pooley
// 
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// version 2 as published by the Free Software Foundation.
// 
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License (gpl.txt) for more details. 
// 
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
// 
// You can contact the author, Nathan (Acorn) Pooley, by writing
// to Nathan (Acorn) Pooley, 949 Buckeye Drive, Sunnyvale, CA  94086, USA
// or through the form at http://www.rawbw.com/~acorn/wand/feedback.html
//

//#@DOC@ Generate a square root lookup table & test wand sqrt algorithm



#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "ac_assert.h"

#define TABLE_INDEX_BITS    4

int *g_table = 0;
void ref(void)
{
    int i;
    for (i=1; i<32; i++) {
        printf(" sqrt(%02x)=%04x = %f\n",
            i,
            (int)(sqrt(i)*256),
            sqrt(i));
    }
}


//
// 3 tables:
// input 01000000 00000000 - 01111111 11111111
// input 10000000 00000000 - 10111111 11111111
// input 11000000 00000000 - 11111111 11111111
//                             x--> 
// cnt = number of bits in table starting from x-->
//
void tblgen(int bitCnt)
{
    static int t[2048];
    int idx = 0;
    int i;
    int step = 0x4000 >> bitCnt;

    printf("Sqrt table for %d index bits\n",bitCnt);
    AC_ASSERT(bitCnt > 0);
    AC_ASSERT(bitCnt < 8);
    for (i=0x4000; i<0xffff; i+=step) {
        double v = i;
#if TABLE_INDEX_BITS==4
        v += 6; // 0.57647705
#endif
#if TABLE_INDEX_BITS==5
        v += 3; //
#endif
        v = sqrt(v);
        t[idx] = v;
        printf(" t[%02x] = sqrt(%04x) = %04x = %12.5f\n",idx,i,t[idx],v);
        idx++;
    }
    AC_ASSERT(idx == 3<<bitCnt);
    printf("\n\n");
    g_table = t;
}

int my_sqrt16(unsigned int val)
{
    int valh = (val >> 8) & 0xff;
    int vall = (val     ) & 0xff;
    int cnt = 0;
    int *tbl;
    int res;
    if (!g_table) tblgen(TABLE_INDEX_BITS);

    //printf("sqrt(0x%04x) ",val);

    if (!valh) {
        cnt += 8;
        valh = vall;
        vall = 0;
        if (!valh) {
            //printf(" = 0\n");
            return 0;
        }
    }
    if (!(valh & 0xf0)) {
        cnt  += 4;
        valh  = (valh << 4) & 0xf0;
        valh |= (vall >> 4) & 0x0f;
        vall  = (vall << 4) & 0xf0;
    }
    if (!(valh & 0xc0)) {
        cnt += 2;
        valh  = (valh << 2) & 0xfc;
        valh |= (vall >> 6) & 0x03;
        //vall  = (vall << 2) & 0xfc;
    }
    //printf("valh = %02x",valh);
    
#if 0
#if TABLE_INDEX_BITS == 4
    if ((valh & 0xfc) != 0xfc) {
        valh += 0x02;
    }
#endif

#if TABLE_INDEX_BITS == 5
    if ((valh & 0xfe) != 0xfe) {
        valh += 0x01;
    }
#endif
#else

#if TABLE_INDEX_BITS == 4
    valh += 0x02;
#endif

#if TABLE_INDEX_BITS == 5
    valh += 0x01;
#endif
#endif
    tbl = g_table;

    AC_ASSERT((cnt&1)==0);
    AC_ASSERT(cnt >= 0);
    AC_ASSERT(cnt < 16);

    if (valh & 0x100) {
        if (cnt) {
            res = 0x100 >> (cnt/2);
        } else {
            res = 0xff;
        }
    }  else {
        if (!(valh & 0x40)) {
            AC_ASSERT((valh & 0xc0) == 0x80);
            tbl += 1<<TABLE_INDEX_BITS;
        } else if (valh & 0x80) {
            AC_ASSERT((valh & 0xc0) == 0xc0);
            tbl += 2<<TABLE_INDEX_BITS;
        } else {
            AC_ASSERT((valh & 0xc0) == 0x40);
        }
        AC_ASSERT(TABLE_INDEX_BITS <= 6);
        valh &= 0x3f;
        valh >>= 6-TABLE_INDEX_BITS;
        res = tbl[valh];

        res >>= cnt/2;
    }

    //printf("\n");

    return res;
}


int main(int argc, char *argv[])
{
    int i;
    int maxerr = 0;
    double errsum = 0;
    double cnt = 0;

    //tblgen(TABLE_INDEX_BITS);
    //exit(0);
    my_sqrt16(1);
    printf("==================\n");
    //ref();
    for (i=0; i<65536; i++) {
        int r = my_sqrt16(i);
        int c = (int)sqrt(i);
        int err = r-c;
        if (err<0) err = -err;
        if (maxerr<err) {
            maxerr = err;
        }
        cnt += 1;
        errsum += err;
        printf(" i=%04x  sqrt16=%02x  sqrt=%02x   err=%-3d  maxerr=%-3d\n",
            i,
            r,
            c,
            err,
            maxerr);
    }
    printf("maxerr=%d  cnt=%d  avgerr=%12.8f\n",
        maxerr,
        (int)cnt,
        errsum/cnt);

    return 0;
}