//
// parse.c - OBSOLETE read a wpic*.txt file (stdin) and parse into motion codes (stdout)
//
// 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@ OBSOLETE read a wpic*.txt spell file (stdin) and parse into motion tokens

//
#include <stdio.h>
#include <string.h>
#include <ctype.h>

#include "tokens.h"

int OPT_quiet = 1;
int OPT_hex = 1;

#define DEBUG   0

#define CNT_MAX 1024


#define ABS(a) ((a)<0?-(a):(a))


#define TH_T_STILL      0.25
#define TH_A_JUMP       5
#define TH_A_NEWQUAD    7
#define TH_A_SNAPQUAD   6
#define TH_A_CONNECT    2

static char *shapeStr[] = {
    CK_TOKSHAPE_STRINGS()
};
static char *quadStr[] = {
    "down ",
    "right",
    "up   ",
    "left ",
};

int adiff(int a1, int a0)
{
    int a = a1 - a0;
    a &= 0x1f;
    if (a & 0x10) a -= 0x20;
    return a;
}

int a2quad(int a)
{
    return ((a+4) >> 3) & 3;
}

void look(
        float *ti, 
        float *xi, 
        float *yi, 
        int cnti)
{
    //static float scale_a = 1.0;
    double max_sum = 0;
    double max_ydt = 0;

    int r_bound[CNT_MAX];
    int r_quad[CNT_MAX];
    int r_shape[CNT_MAX];
    int r_ccw[CNT_MAX];
    float r_time[CNT_MAX];
    float r_sloperatio[CNT_MAX];
    int r_flags[CNT_MAX];
    int r_val[CNT_MAX];

    int bcnt = 0;
    //int c=0;
    int i;
    int cura0 = 0;
    int cura1 = 0;
    float timea0 = 0;
    float ly = 0;
    int lx = 0;

    float cross_t = 0;  // valid if cross_dir != 0
    int cross_i = -1;   // valid if cross_dir != 0
    int cross_dir = 0;  

    int is_new = 1;
    int is_jump = 1;

    //
    // find region boundaries
    //
    for (i=0; i<cnti-1; i++) {
        float y = yi[i];
        int x = xi[i] + 0.5;
        float dt = ti[i+1]-ti[i];
        float ydt = y * dt;
        int da0;
        int da1;
        int dx = 0;
        int reason = 0;

        da0 = adiff(x, cura0);

        if (max_ydt < ydt) {
            max_ydt = ydt;
        }

        if (is_new) goto do_new;

        dx = adiff(x,lx);

        //
        // zero mag?
        //
        if (y<1) {
            if (ly < 1) {
                continue;
            } else if (dt > TH_T_STILL) {
                is_new = 1;
                reason = 1;
            } else {
                continue;
            }
        }
        if (ly < 1) {
            reason = 2;
            is_new = 1;
        }

        if (is_new) goto do_new;

#if 1
        //
        // do not know which quad we are starting in?
        //
        if (cura0 != cura1) {

            da1 = adiff(x,cura1);

            if (da0 > TH_A_SNAPQUAD || da0 <= -TH_A_SNAPQUAD) {
                if (da1 > TH_A_SNAPQUAD || da1 <= -TH_A_SNAPQUAD) {
                    is_new = 1;
                    reason = 3;
#if DEBUG
                    printf("--A\n");
#endif
                } else {
                    cura0 = cura1;
                    da0 = da1;
#if DEBUG
                    printf("--B\n");
#endif
                }
            } else if (da1 > TH_A_SNAPQUAD || da1 <= -TH_A_SNAPQUAD) {
                cura1 = cura0;
#if DEBUG
                printf("--C\n");
#endif
            } else if (ABS(da0) > ABS(da1)) {
                timea0 -= ydt;
#if DEBUG
                printf("--D  %12.5f  %12.5f\n",ydt,timea0);
#endif
            } else if (ABS(da0) < ABS(da1)) {
                timea0 += ydt;
#if DEBUG
                printf("--E  %12.5f  %12.5f\n",ydt,timea0);
#endif
            }
        }
#endif
        if (is_new) goto do_new;

        //
        // big jump?
        //
        if (ABS(dx) > TH_A_JUMP) {
            cross_dir = 0;
            is_new = 1;
            reason = 4;
            is_jump = 1;
        }

        if (is_new) goto do_new;

        //
        // moved into another quad?
        //
        if (cura0 == cura1) {
            if (da0 > TH_A_NEWQUAD || da0 <= -TH_A_NEWQUAD) {
#if DEBUG
                printf("--F da0=%3d  x=%3d   th=%3d\n",da0,x,TH_A_NEWQUAD);
#endif
                is_new = 1;
                reason = 5;
            }
            if (da0 >= 0 && cross_dir < 0) {
                cross_dir = 0;
            }
            if (da0 < 0 && cross_dir > 0) {
                cross_dir = 0;
            }
            if (da0 > 3) {
                if (cross_dir <= 0) {
                    cross_dir = 1;
                    cross_i   = i;
                    cross_t   = 0;
                }
                cross_t += ydt;
            } else if (da0 < -4) {
                if (cross_dir >= 0) {
                    cross_dir = -1;
                    cross_i   = i;
                    cross_t   = 0;
                }
                cross_t += ydt;
            } else if (cross_dir) {
                cross_t -= ydt;
                if (cross_t < 0) {
                    cross_dir = 0;
                }
            }
        }

        do_new:

        if (i==0 || i==cnti-2 || is_new) {
            if (cross_dir) {
                i = cross_i;
            }
            x = xi[i] + 0.5;
            y = yi[i];
            r_bound[bcnt] = i;
            r_quad[bcnt] = a2quad(x);
            if (y < 1 ) r_quad[bcnt] = -1;
#if 1
            //
            // find most representative quad
            //
            if (bcnt>0) {
                float sum[4];
                int i2;
                int q2;
                int nonz = 0;
                memset(sum,0,sizeof(sum));
                for (i2=r_bound[bcnt-1]; i2<r_bound[bcnt]; i2++) {
                    float y2 = yi[i2];

                    if (y2 >= 1) {
                        int x2 = xi[i2] + 0.5;
                        float dt2 = ti[i2+1]-ti[i2];
                        float ydt2 = y2 * dt2;
                        q2 = a2quad(x2);
                        sum[q2] += ydt2;
                        nonz = 1;
                    }
                }
                if (nonz) {
                    q2 = r_quad[bcnt-1];
                    q2 = q2<0 ? 0 : q2;
                    for (i2=0; i2<4; i2++) {
                        if (sum[i2] > sum[q2]) {
                            q2 = i2;
                            if (max_sum < sum[i2]) {
                                max_sum = sum[i2];
                            }
                        }
                    }
                    r_quad[bcnt-1] = q2;
                } else {
                    r_quad[bcnt-1] = -1;
                }

                //
                // collapse repeated sections
                //
                while(bcnt>1 && r_quad[bcnt-1]==r_quad[bcnt-2]) {
                    if (!OPT_quiet) {
                        printf("Collapsing t=%12.5f - %12.5f\n",
                            ti[r_bound[bcnt-2]],
                            ti[i]);
                    }
                    r_bound[bcnt-1] = i;
                    r_quad[bcnt-1]  = r_quad[bcnt];
                    bcnt--;
                }
            }
#endif

            if (y >= 1) {
                cura0 = r_quad[bcnt]*8;
                cura1 = cura0;
                if (is_jump || ABS(dx) > TH_A_CONNECT) {
                    dx = adiff(x,cura0);
                    if (dx < 0) {
                        cura1 = ((r_quad[bcnt]+3)&3)*8;
                    } else {
                        cura1 = ((r_quad[bcnt]+1)&3)*8;
                    }
                }
            }
#if DEBUG
            if (bcnt>0) {
                printf("bound[%2d]=%2d  t=%12.5f  q=%2d\n",
                    bcnt-1,
                    r_bound[bcnt-1],
                    ti[r_bound[bcnt-1]],
                    r_quad[bcnt-1]);
            }
            printf("bound[%2d]=%2d  t=%12.5f  q=%2d  y=%3d x=%3d  "
                "cura=%2d/%2d  r=%d\n",
                bcnt,
                i,
                ti[i],
                r_quad[bcnt],
                (int)y,
                x,
                cura0,
                cura1,
                reason);
#endif

            bcnt++;

            cross_dir = 0;
            is_new = 0;
            is_jump = 0;
        }

        ly = y;
        lx = x;
    }
    
    {
    int j;
    float s_time = 0;
    float s_dt_min = 0;
    float s_dt_max = 0;
    int is_inc = 0;
    int is_dec = 0;
    int is_every = 0;
    int is_pause = 0;
    int is_post_jump = 0;
    int is_pre_jump = 1;
    int is_post_connect = 0;
    int is_pre_connect = 1;
    int shapeBits = 0;
    int s_cnt = 0;
    int pre_qdir = 0;
    int post_qdir = 0;

    j=0;
    for (i=0; i<cnti-1; i++) {
        float y = yi[i];
        int x = xi[i] + 0.5;
        float dt = ti[i+1]-ti[i];
        //float ydt = y * dt;
        int dx;

        if (r_bound[j] == i) {
            if (j) {

                //
                // jump follows this?
                //
                is_post_connect = 0;
                if (is_pause) {
                    is_post_jump = 1;
                } else if (y<1 && dt > TH_T_STILL) {
                    is_post_jump = 1;
                } else {
                    int ii = i;
                    while(1) {
                        if (ii>=cnti || j>=bcnt || ii==r_bound[j+1]) {
                            is_post_jump = 1;
                            break;
                        }
                        if (yi[ii]>=1) {
                            dx = adiff(((int)(xi[ii]+0.5)),lx);
                            if (ABS(dx)>7) {
                                is_post_jump = 1;
                            } else {
                                is_post_jump = 0;
                                if (ABS(dx) <= TH_A_CONNECT) {
                                    is_post_connect = 1;
                                }
                            }
                            break;
                        }
                        ii++;
                    }
                }
                shapeBits = 0;
                if (!is_pre_jump && j>1 && r_quad[j-1]>=0 && r_quad[j-2]>=0) {
                    pre_qdir = r_quad[j-1] - r_quad[j-2];
                    pre_qdir &= 3;
                    if (pre_qdir == 1) shapeBits = 1;
                    if (pre_qdir == 3) shapeBits = 2;
                }
                if (!is_post_jump && j<bcnt && r_quad[j]>=0 && r_quad[j-1]>=0) {
                    post_qdir = r_quad[j] - r_quad[j-1];
                    post_qdir &= 3;
                    if (post_qdir == 1) shapeBits |= 4;
                    if (post_qdir == 3) shapeBits |= 8;
                }
#if 0
printf("bits=%02x  jump=%d/%d qdir=%d/%d  quad=%d/%d/%d\n",
    shapeBits,is_pre_jump,is_post_jump,
    pre_qdir,post_qdir,
    r_quad[j-2],r_quad[j-1],r_quad[j]);
#endif

                r_time[j-1]  = s_time;
                r_ccw[j-1]   = 0;
                if (is_pause) {
                    r_shape[j-1] = CK_TOKSHAPE_PAUSE;
                    is_inc = 0;
                    is_dec = 0;
                    is_every = 0;
                    is_pre_connect = 0;
                    is_post_connect = 0;
                } else switch(shapeBits) {
                    case 0x5:   // + +
                        r_shape[j-1] = CK_TOKSHAPE_CIRCLE;
                        r_ccw[j-1]   = 1;
                        break;
                    case 0xa:   // - -
                        r_shape[j-1] = CK_TOKSHAPE_CIRCLE;
                        break;

                    case 0x6:   // - +
                        r_shape[j-1] = CK_TOKSHAPE_REVERSE;
                        r_ccw[j-1]   = 1;
                        break;
                    case 0x9:   // + -
                        r_shape[j-1] = CK_TOKSHAPE_REVERSE;
                        break;

                    case 0x0:   // 0 0
                        r_shape[j-1] = CK_TOKSHAPE_FLAT;
                        break;

                    case 0x1:   // + 0
                        r_shape[j-1] = CK_TOKSHAPE_STOP;
                        r_ccw[j-1]   = 1;
                        break;
                    case 0x2:   // - 0
                        r_shape[j-1] = CK_TOKSHAPE_STOP;
                        break;
                    case 0x4:   // 0 +
                        r_shape[j-1] = CK_TOKSHAPE_START;
                        r_ccw[j-1]   = 1;
                        break;
                    case 0x8:   // 0 -
                        r_shape[j-1] = CK_TOKSHAPE_START;
                        break;

                    case 0x3:
                    case 0x7:
                    case 0xb:
                    case 0xc:
                    case 0xd:
                    case 0xe:
                    case 0xf:
                    default:
                        r_shape[j-1] = CK_TOKSHAPE_PAUSE;
                        break;
                }

#if 0
                if (!is_pause) {
                    float sum[4];
                    int i2;
                    int q2;
                    memset(sum,0,sizeof(sum));
                    for (i2=r_bound[j-1]; i2<r_bound[j]; i2++) {
                        float y2 = yi[i2];
                        int x2 = xi[i2] + 0.5;
                        float dt2 = ti[i2+1]-ti[i2];
                        float ydt2 = y2 * dt2;
                        q2 = a2quad(x2);

                        sum[q2] += ydt2;
                    }
                    q2 = r_quad[j-1];
                    for (i2=0; i2<4; i2++) {
                        if (sum[i2] > sum[q2]) {
                            q2 = i2;
                            if (max_sum < sum[i2]) {
                                max_sum = sum[i2];
                            }
                        }
                    }
                    if (q2 != r_quad[j-1]) {
                        r_quad[j-1] = q2;
                    }
                }
#endif

                r_flags[j-1] = 0;
                if (!is_pause) {
                    r_flags[j-1] |= 
                        (is_pre_connect?1:0) |
                        (is_every?2:0)|
                        (is_inc|is_dec?4:0)|
                        (is_post_connect?8:0);
                }
            
#if 1
                r_val[j-1] = 
                    CK_MAKE_TOKEN(r_shape[j-1],r_quad[j-1],r_ccw[j-1]);
#else
                r_val[j-1] = r_shape[j-1]<<4;
                if (!is_pause) {
                    r_val[j-1] |= r_quad[j-1];
                    if (r_ccw[j-1]) {
                        r_val[j-1] |= 0x4;
                    }
                }
#endif

#if DEBUG
                printf("%12.5f RESULT[%2d]=%02x shape=%d=%s  "
                        "%s%s%s%s quad=%d=%s  "
                        "ccw=%d=%s  time=%12.5f ",
                    ti[r_bound[j-1]],
                    j-1,
                    r_val[j-1],
                    r_shape[j-1],
                    shapeStr[r_shape[j-1]],
                    (r_flags[j-1]&1)?"C":" ",
                    (r_flags[j-1]&2)?"E":" ",
                    (r_flags[j-1]&4)?"M":" ",
                    (r_flags[j-1]&8)?"C":" ",
                    r_quad[j-1],
                    r_shape[j-1]==CK_TOKSHAPE_PAUSE ?  "-    " : quadStr[r_quad[j-1]],
                    r_ccw[j-1],
                    r_shape[j-1]==CK_TOKSHAPE_PAUSE ?  "-  " : r_ccw[j-1]?"ccw":"cw ",
                    r_time[j-1]);
#endif

                r_sloperatio[j-1] = 0;
                if (r_shape[j-1] != CK_TOKSHAPE_PAUSE &&
                    r_shape[j-1] != CK_TOKSHAPE_FLAT) {
                    r_sloperatio[j-1] = s_dt_min/s_dt_max;
#if DEBUG
                    printf("dt=%.5f/%.5f=%6.2f",
                        s_dt_min,
                        s_dt_max,
                        r_sloperatio[j-1]);
#endif
                }

#if DEBUG
                printf("\n");
#endif

                is_pre_jump    = is_post_jump;
                is_pre_connect = is_post_connect;
            } else {
                is_pre_jump    = 1;
                is_pre_connect = 0;
            }

            is_inc = 1;
            is_dec = 1;
            is_every = 1;
            is_pause = 1;
            s_cnt = 0;
            s_time = 0;
            s_dt_min = 0.0;
            s_dt_max = 0.0;
            j++;
        }

        s_time += dt;

        if (y<1) {
            continue;
        }

        is_pause = 0;

        s_dt_min = dt < s_dt_min ? dt : s_dt_min;
        s_dt_max = dt > s_dt_max ? dt : s_dt_max;

        if (s_cnt) {
            dx = adiff(x,lx);
            if (dx>0) {
                is_dec = 0;
            } else if (dx<0) {
                is_inc = 0;
            }
            if (ABS(dx)>2) {
                is_every = 0;
            }
        } else {
            s_dt_min = dt;
            s_dt_max = dt;
        }

        s_cnt++;
        lx = x;
        ly = y;
    }

    }

    while(bcnt > 0) {
        if (r_shape[bcnt-1] == CK_TOKSHAPE_PAUSE) {
            bcnt--;
            continue;
        }
        if (r_quad[bcnt-1] == 1 || r_quad[bcnt-1] == 3) {
            bcnt--;
            continue;
        }
        break;
    }

    if (!OPT_quiet) {
        printf("=======================================\n");
        int j;
        for(j=0; j<bcnt; j++) {
            printf("%12.5f RESULT[%2d]=%02x %d=%s  "
                    "%s%s%s%s %d=%s  "
                    "%d=%s  time=%12.5f ",
                ti[r_bound[j]],
                j,
                r_val[j],
                r_shape[j],
                shapeStr[r_shape[j]],
                (r_flags[j]&1)?"C":" ",
                (r_flags[j]&2)?"E":" ",
                (r_flags[j]&4)?"M":" ",
                (r_flags[j]&8)?"C":" ",
                r_quad[j],
                r_shape[j]==CK_TOKSHAPE_PAUSE ?  "-    " : quadStr[r_quad[j]],
                r_ccw[j],
                r_shape[j]==CK_TOKSHAPE_PAUSE ?  "-  " : r_ccw[j]?"ccw":"cw ",
                r_time[j]);

            if (r_shape[j] != CK_TOKSHAPE_PAUSE &&
                r_shape[j] != CK_TOKSHAPE_FLAT) {
                printf("dtr=%6.2f",r_sloperatio[j]);
            }

            printf("\n");
        }

        printf("max_ydt=%12.3f  max_sum=%12.3f\n",
                max_ydt,
                max_sum);
    }
    if (0) {
        int first = 1;
        int j;
        for(j=0; j<bcnt; j++) {
            if (first && r_shape[j]==CK_TOKSHAPE_PAUSE) continue;
            first = 0;
            printf("%02x ",r_val[j]);
        }
        printf("\n");
    }
    if (!OPT_quiet || !OPT_hex) {
        int first = 1;
        int j;
        for(j=0; j<bcnt; j++) {
            if (first && r_shape[j]==CK_TOKSHAPE_PAUSE) continue;
            first = 0;
            printf("%c%c%c ",
                shapeStr[r_shape[j]][0],
                r_ccw[j]?'+':'-',
                r_shape[j]==CK_TOKSHAPE_PAUSE ?  ' ' : quadStr[r_quad[j]][0]);
        }
        printf("\n");
    }
    if (!OPT_quiet || OPT_hex) {
        int first = 1;
        int j;
        for(j=0; j<bcnt; j++) {
            if (first && r_shape[j]==CK_TOKSHAPE_PAUSE) continue;
            first = 0;
            printf("%02x ",r_val[j]);
        }
        printf("\n");
    }

}

void fixtime(float *to, float *ti, int cnt)
{
    int i;
    float lt = 0;
    float t0 = ti[0];
    for (i=0; i<cnt; i++) {
        float t = ti[i] - t0;
        while(t < lt) {
            t0 -= 65536;
            t = ti[i] - t0;
        }
        lt = t;
        to[i] = t * 1.0/(16.0 * 256);
    }
}

int getline(FILE *fp, float *t, float *x, float *y)
{
    int c;

    while(1) {
        c = getc(fp);
        while(isspace(c)) {
            c = getc(fp);
        }
        if (c=='#') {
            while(c!='\n' && c!=EOF) {
                c = getc(fp);
            }
            continue;
        }
        if (c==EOF) return 0;
        ungetc(c,fp);
        fscanf(fp,"%f %f %f",t,x,y);
        return 1;
    }

}

int get(FILE *in, float *t, float *x, float *y)
{
    int i;
    for(i=0; i<CNT_MAX && getline(in,t+i,x+i,y+i); i++);
    return i;
}

int main(int argc, char *argv[])
{
    float ti[CNT_MAX], xi[CNT_MAX],yi[CNT_MAX];
    float ti2[CNT_MAX];
    int cnti=0;

    if (argc > 1) {
        printf("%s: ",argv[1]);
    }

    cnti = get(stdin,ti,xi,yi);
    if (cnti) {
        fixtime(ti2,ti,cnti);
        look(ti2,xi,yi,cnti);
    }
    return 0;
}