#!/usr/bin/perl -w
#
# ortho_encoder.pm - Ortho of Borg Encoder Support Routines
#

package ortho_encoder;
require Exporter;
@ISA    = qw(Exporter);
@EXPORT = qw(inches_to_encoder_counts encoder_counts_to_inches encoder_open encoder_close encoder_read);

use POSIX;
use strict;

my $ENCODER_PORT = '/dev/ttyS1';

my $ENCODER_TICKS_PER_INCH = 303;

sub inches_to_encoder_counts($) {
    return shift(@_) * $ENCODER_TICKS_PER_INCH;
}

sub encoder_counts_to_inches($) {
    return shift(@_) / $ENCODER_TICKS_PER_INCH;
}

sub encoder_cbreak($) {
    my $encoder_fd = shift(@_);
    my ($termios, $c_iflag, $new_iflag, $c_lflag, $new_lflag);
    my ($c_oflag, $new_oflag, $c_cflag, $new_cflag);
    $termios = POSIX::Termios->new;
    $termios->getattr($encoder_fd) or die "$! in getattr";

#    print $termios->getispeed, " ", $termios->getospeed, " ", POSIX::B9600, " ", POSIX::B38400, "\n";
    $termios->setispeed(POSIX::B38400);
    $termios->setospeed(POSIX::B38400);

    $c_iflag = $termios->getiflag;
    $new_iflag = ($c_iflag & ~(BRKINT | PARMRK | ISTRIP |
			       INLCR | IGNCR | ICRNL | IXON | IXOFF)) | IGNBRK;

    $c_lflag = $termios->getlflag;
    $new_lflag = $c_lflag & ~(ECHO | ECHOK | ICANON | ISIG | IEXTEN);

    $c_oflag = $termios->getoflag;
    $new_oflag = $c_oflag & ~(OPOST);

    $c_cflag = $termios->getcflag;
    $new_cflag = ($c_cflag  & ~(CSIZE | PARENB )) | CS8;
    $termios->setiflag($new_iflag);
    $termios->setlflag($new_lflag);
    $termios->setoflag($new_oflag);
    $termios->setcflag($new_cflag);
    $termios->setcc(VTIME,1);
    $termios->setattr($encoder_fd, &POSIX::TCSANOW);
}

my ($encoder_fd);

sub encoder_open() {
    system("setserial", $ENCODER_PORT, "spd_vhi");
    $encoder_fd = POSIX::open($ENCODER_PORT, O_RDWR | O_NOCTTY | O_NONBLOCK) 
	or die "$! in open";
    encoder_cbreak($encoder_fd);
}

sub encoder_close() {
    POSIX::close($encoder_fd);
}

sub encoder_read() {
    my ($buff, $encoder_buff, $encoder_count, $ret, $speed);
    my $encoder_in = '';
    my $encoder_out;
    vec($encoder_in, $encoder_fd, 1) = 1;

    POSIX::tcflush($encoder_fd, &POSIX::TCIOFLUSH);
    $buff = 'E';
#    print ">"; 
    POSIX::write($encoder_fd, $buff, 1);
    if (select ($encoder_out=$encoder_in, undef, undef, 0.1) == 1) {
	while (!defined ($ret = POSIX::read($encoder_fd,$encoder_buff, 4))) {
#            print "<";
	};

	$encoder_count = unpack("l", $encoder_buff);
	return $encoder_count;
    }
    else {
	return undef;
    }
};

return 1;