/* $Id: libesketch.c,v 1.2 2007/05/24 18:20:43 Kevin Bacon Exp $ */
/* A program to drive a couple of stepper motors */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/io.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <math.h>
#include <fcntl.h>
#include <termios.h>
#include <string.h>
#include <errno.h>
#include <linux/ppdev.h>
#include <linux/parport.h>
#include <sys/ioctl.h>
#include "libesketch.h"

/* One coil energised */
/*int sequence[ 4 ] = { 0x1, 0x2, 0x4, 0x8 };*/

/* Two coils energised (more torque) */
int sequence[ 4 ] = { 0x3, 0x6, 0xc, 0x9 };

/* Previous coordinates.  External so they can be initialized */
int old_x = 0;
int old_y = 0;

/* serial port fd */
int serial_fd;

/* amount wires stretch */
int stretch = 8;

/* usb stuff */
unsigned char sequence_array[1024];
int usb_fd;

void send_serial( char *s )
{
    int i, j, status;
    char t[32];

    printf( "%s", s );
    tcflush( serial_fd, TCIFLUSH );
    usleep( 10000 );
    write( serial_fd, s, strlen(s));
    tcdrain( serial_fd );
    for( j = 0; j < 10000; j++ )
    {
        status = read( serial_fd, t, 32 );
        if( status > 0 ) break;
        usleep( 100 );
    }
    printf( "status=%d,j=%d", status, j );
    for( i = 0; i < status; i++ ) printf( ",%d", t[i] );
    printf( "\n" );
}


void reset( int x, int y )
{
    old_x = x;
    old_y = y;
}

/* Steps motors.  Difference must be a maximum of one step */
void stepto( int x, int y )
{
    char i;
    int dx = x - old_x;
    int dy = y - old_y;
    // char s[20];

    if( abs( dx ) > 1 || abs( dy ) > 1 )
    {
	printf( "Error: stepto step size (%d, %d) > 1!\n:", dx, dy );
	printf( "Press Enter to continue\n" );
	getchar();
    }

    /* Save the old coordinates */
    old_x = x;
    old_y = y;

    /* Combine x and y sequences */
    i = ( sequence[ x & 3 ] ) | ( sequence[ y & 3 ] << 4 );

    // Send serial commands
    // sprintf( s, "X%+dSY%+dSI\n", dx * 16, dy * 16 );
    // send_serial( s );

#if 0
    // Send to uss720 driver
    if( ioctl( usb_fd, PPWDATA, &i )) perror( "PPWDATA" );
    usleep( 2000 );
#endif
#if 0
    // Send usb
    if( usb_fd >= 0 )
    {
        int j;

        for( j = 0; j < 1024; j++ ) sequence_array[j] = i;
        j = write( usb_fd, sequence_array, 1024 );
        if( j < 0 )
        {
            printf( "usb_fd=%d, status=%d, errno=%d\n", usb_fd, j, errno );
            perror( "" );
        }
    }
#endif
#if 1
    outb( i, 0x3bc );
    outb( i, 0x378 );
    outb( i, 0x278 );
    usleep( 2000 );
#endif
}

void drawto_simple( int x, int y )
{
    float dx, dy, i, j;

    /* Calculate deltas */
    dx = x - old_x;
    dy = y - old_y;

    if( fabs( dx ) < fabs( dy ))
    {
        dx /= fabs( dy );
        if( dy > 0 )
        {
            for( i = old_x, j = old_y; j < y; i += dx, j += 1.0 )
            {
                stepto( i, j );
            }
        }
        else
        {
            for( i = old_x, j = old_y; j > y; i += dx, j -= 1.0 )
            {
                stepto( i, j );
            }
        }
    }
    else
    {
        dy /= fabs( dx );
        if( dx > 0 )
        {
            for( i = old_x, j = old_y; i < x; i += 1.0, j += dy )
            {
                stepto( i, j );
            }
        }
        else
        {
            for( i = old_x, j = old_y; i > x; i -= 1.0, j += dy )
            {
                stepto( i, j );
            }
        }
    }

    stepto( x, y );
}

void drawto_stretch( int x, int y )
{
    static int xa, xb;
    static int ya, yb;
    int dx, dy;

    // Init
    if( xa < 0 )
    {
        xa = xb = x;
        ya = yb = y;
    }

    if(     ( x - xa ) > 0 ) dx =  stretch;
    else if(( x - xa ) < 0 ) dx = -stretch;
    else                     dx =  0;

    if(     ( y - ya ) > 0 ) dy =  stretch;
    else if(( y - ya ) < 0 ) dy = -stretch;
    else                     dy =  0;

    drawto_simple( x + dx, y + dy );
    drawto_simple( x, y );

    // Save two previous copies
    xb = xa;
    yb = ya;
    xa = x;
    ya = y;
}

// Draw everything 3 times
void drawto_multi( int x, int y )
{
    static int save_x = -1, save_y;

    // Init
    if( save_x < 0 )
    {
        save_x = x;
        save_y = y;
    }

    drawto_stretch( x, y );
    drawto_stretch( save_x, save_y );
    drawto_stretch( x, y );

    // Save x, y;
    save_x = x;
    save_y = y;
}

// Break drawto into small steps
void drawto_steps( int x, int y )
{
    int ax, ay;
    int dx, dy;
    int step = 5;

    while( 1 )
    {
        dx = x - old_x;
        dy = y - old_y;

        if( fabs( dx ) <= step && fabs( dy ) <= step ) break;

        ax = old_x;
        ay = old_y;

        if(      dx > 0 ) ax += MIN(  dx, step );
        else if( dx < 0 ) ax -= MIN( -dx, step );
        if(      dy > 0 ) ay += MIN(  dy, step );
        else if( dy < 0 ) ay -= MIN( -dy, step );

        drawto_stretch( ax, ay );
    }
    drawto_stretch( x, y );
}

/* Point drawto at specific function */
void drawto( int x, int y )
{
    static int init = 0;
    // char s[32];

    // Assume we are already at start position
    // This eliminates initial line from 0,0 to start position
    if( !init )
    {
        reset( x, y );
        init = 1;
        return;
    }

#if 1
    drawto_steps( x, y );
#else
    // Send serial commands
//    sprintf( s, "x%dgy%dg", x * 16, y * 16 );
//    tcflush( serial_fd, TCIFLUSH );
//    write( serial_fd, s, strlen( s ));
//    tcdrain( serial_fd );

    send_serial( "x\n" );
    sprintf( s, "%dg\n", x * 16 );
    send_serial( s );
    send_serial( "y\n" );
    sprintf( s, "%dg\n", y * 16 );
    send_serial( s );
    send_serial( "i" );
#endif
}

void init()
{
    // int i;
    // struct termios newtio;

    // Allow hw access
    iopl( 3 );

#if 0
    // Open serial port
    serial_fd = open( "/dev/ttyS0", O_RDWR | O_NOCTTY );

    bzero( &newtio, sizeof(newtio) ); /* clear struct for new port settings */
    newtio.c_cflag = B9600 | CS8 | CLOCAL | CREAD;
    newtio.c_oflag = 0;

    tcflush( serial_fd, TCIFLUSH );
    cfmakeraw( &newtio );
    tcsetattr( serial_fd, TCSANOW, &newtio );

    // full step mode
    // write( serial_fd, "64!2o0v", 7 );
    write( serial_fd, "2o0v", 7 );

    // Open usb port
//    usb_fd = open( "/dev/usb/lp0", O_RDWR );
    usb_fd = open( "/dev/parport3", O_RDWR );
    if( usb_fd < 0 )
    {
        printf( "Can't open /dev/usb/lp0\n" );
    }

    if( ioctl( usb_fd, PPCLAIM )) perror( "PPCLAIM" );

//    i = IEEE1284_MODE_BYTE;
//    if( ioctl( usb_fd, PPNEGOT, &i )) perror( "PPNEGOT" );

    i = 0;
    if( ioctl( usb_fd, PPDATADIR, &i )) perror( "PPDATADIR" );
#endif
}

void stop()
{
    // char i = 0;

    /* Turn off motor drivers */
    outb( 0, 0x3bc );
    outb( 0, 0x378 );
    outb( 0, 0x278 );

#if 0
    if( ioctl( usb_fd, PPWDATA, &i )) perror( "PPWDATA" );
#endif
}