/* A program to drive a couple of stepper motors */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "libesketch.h"

#define NPOINTS 10000
#define POINT_DONE   0
#define POINT_START  1
#define POINT_MIDDLE 2
#define POINT_END    3

struct point
{
    int x;
    int y;
    int flags;
};

int main( int argc, char **argv )
{
    int i;
    int c;
    int comment = 0;
    int x, y;
    struct point *points;
    int current_point = 0;
    int dist, min;
    int min_i;
    int down = 0;
    int sort = 0;
    int yflip = 0;
    
    points = malloc( NPOINTS * sizeof( struct point ));
    if( points == NULL )
    {
        printf( "Error: Can't allocate %d points\n", NPOINTS );
        exit( 1 );
    }

    // Allow hw access
    init();

    printf( "Usage: flut [sort] [yflip] < file.txt\n" );
    if( argc > 1 )
    {
        if( strcmp( argv[1], "sort" ) == 0 ) sort = 1;
        if( strcmp( argv[1], "yflip" ) == 0 ) yflip = 1;
    }
    if( argc > 2 )
    {
        if( strcmp( argv[2], "sort" ) == 0 ) sort = 1;
        if( strcmp( argv[2], "yflip" ) == 0 ) yflip = 1;
    }

    x = y = 0;
    while( 1 )
    {
        c = getchar();
        if( c == EOF ) break;
        else if( c == '%' ) { comment = 1; continue; }
        else if( c == '\n' || c == '\r' ) { comment = 0; continue; }
        else if( comment ) continue;
        else if( c == '0' ) { x++;      }
        else if( c == '1' ) { x++; y++; }
        else if( c == '2' ) {      y++; }
        else if( c == '3' ) { x--; y++; }
        else if( c == '4' ) { x--;      }
        else if( c == '5' ) { x--; y--; }
        else if( c == '6' ) {      y--; }
        else if( c == '7' ) { x++; y--; }
        else if( c == 'D' )
        {
            // Mark point as start
            points[ current_point ].x = x;
            points[ current_point ].y = y;
            if( yflip ) points[ current_point ].y = -points[ current_point ].y;
            points[ current_point ].flags = POINT_START;
//printf( "%c (%d,%d) %d\n", c, points[current_point].x, points[current_point].y, points[current_point].flags );
            current_point++;
            down = 1;
            continue;
        }
        else if( c == 'U' )
        {
            // Mark point as end
            points[ current_point ].x = x;
            points[ current_point ].y = y;
            if( yflip ) points[ current_point ].y = -points[ current_point ].y;
            points[ current_point ].flags = POINT_END;
//printf( "%c (%d,%d) %d\n", c, points[current_point].x, points[current_point].y, points[current_point].flags );
            current_point++;
            down = 0;
            continue;
        }
        else continue;

        if( down )
        {
            points[ current_point ].x = x;
            points[ current_point ].y = y;
            if( yflip ) points[ current_point ].y = -points[ current_point ].y;
            points[ current_point ].flags = POINT_MIDDLE;
//printf( "%c (%d,%d) %d\n", c, points[current_point].x, points[current_point].y, points[current_point].flags );
            current_point++;
        }
    }

    if( !sort )
    {
        for( i = 0; i < current_point; i++ )
        {
            // printf( "(%d,%d) %d\n", points[i].x, points[i].y, points[i].flags );
            drawto( points[i].x, points[i].y );
        }
    }
    else
    {
        // Loop until all points drawn
        x = y = 0;
        while( 1 )
        {
            // Find nearest start/end point to current point
            min = 10000 * 10000;
            min_i = -1;
            for( i = 0; i < current_point; i++ )
            {
                if( points[i].flags == POINT_START || points[i].flags == POINT_END )
                {
                     dist = ( points[i].x - x ) * ( points[i].x - x )
                          + ( points[i].y - y ) * ( points[i].y - y );
                     if( dist < min )
                     {
                         min = dist;
                         min_i = i;
                     }
                }
            }

            // Stop when no start/end found
            if( min_i == -1 ) break;

            // Draw this path forwards or backwards
            if( points[min_i].flags == POINT_START )
            {
                while( 1 )
                {
                    x = points[min_i].x;
                    y = points[min_i].y;
                    drawto( x, y );

                    if( points[min_i].flags == POINT_END ) break;
                    points[min_i].flags = POINT_DONE;
                    min_i++;
                }

                // Set the last flag
                points[min_i].flags = POINT_DONE;
            }
            else
            {
                while( 1 )
                {
                    x = points[min_i].x;
                    y = points[min_i].y;
                    drawto( x, y );

                    if( points[min_i].flags == POINT_START ) break;
                    points[min_i].flags = POINT_DONE;
                    min_i--;
                }

                // Set the last flag
                points[min_i].flags = POINT_DONE;
            }
        }
    }

    /* Turn off motor drivers */
    stop();

    return 0;
}