#!/usr/bin/perl -w
#
# SSC SUPPORT ROUTINES
#
# These routines support controlling servos via a Scott Edwards Mini-SSC II.
#

package ssc;
require Exporter;
@ISA    = qw(Exporter);
@EXPORT = qw(ssc_open ssc_close ssc_set_servo ssc_test ir_read);

use POSIX;
use strict;

my $SSC_PORT = '/dev/ttyS2';

my($ssc_fd);

sub ssc_open() {

    $ssc_fd = POSIX::open($SSC_PORT, O_RDWR | O_NOCTTY) or die "$! in open";
    ssc_cbreak($ssc_fd);
}

sub ssc_close {
    POSIX::close($ssc_fd);
}

sub ssc_set_servo($$) {
    my ($servo, $position) = @_;
    my $ret;
    my $ssc_in = '';
    my $ssc_out;
    vec($ssc_in, $ssc_fd, 1) = 1;

    POSIX::tcflush($ssc_fd, &POSIX::TCIOFLUSH);
#    print "ssc=$position\n";
    my $buff = '!1'. chr(3) . chr(255) . chr($servo) . chr($position);
    POSIX::write($ssc_fd, $buff, 6) or die "$! in ssc_set_servo";
    if (select ($ssc_out=$ssc_in, undef, undef, 0.25) == 1) {
	while (!defined ($ret = POSIX::read($ssc_fd,$buff, 1))) {
#            print "<";
	}
	if ($buff !~ ">") {
	    print "\aGARBLED REPLY from SSC: $buff\n";
	}
    }
    else {
	print "\aTIMEOUT: No response from SSC!\n";
    }
}

sub ssc_cbreak($) {
    my $ssc_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($ssc_fd) or die "$! in getattr";

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

    $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($ssc_fd, &POSIX::TCSANOW);
}

sub ssc_test() {
    my ($i);

    for ($i = 0; $i < 256; $i++) {
	ssc_set_servo(0, $i);
	select undef, undef, undef, 0.01
    };
    for ($i = 255; $i >= 0; $i--) {
	ssc_set_servo(0, $i);
	select undef, undef, undef, 0.01
    };
}

sub ir_read($) {
    my $ir_number = shift(@_);

    my $ret;
    my $ssc_in = '';
    my $ssc_out;
    vec($ssc_in, $ssc_fd, 1) = 1;
    my $ir_reading = 0;

    POSIX::tcflush($ssc_fd, &POSIX::TCIOFLUSH);
#    print "ssc=$position\n";
    my $buff = '!A'. $ir_number;
    POSIX::write($ssc_fd, $buff, 3) or die "$! in ir_read";
    do {
	if (select ($ssc_out=$ssc_in, undef, undef, 0.25) == 1) {
	    $buff = '';
	    while (!defined ($ret = POSIX::read($ssc_fd,$buff, 1))) {
#            print "<";
	    }
	    if ($buff =~ /[0123456789]/) {
		$ir_reading = $ir_reading * 10 + $buff;
	    }
	}
	else {
	    print "\aTIMEOUT: No response from IR!\n";
	    return undef;
	}
    } until ($buff =~ ">");

    return $ir_reading;
}   

return 1;