#!/usr/bin/perl -w

use POSIX;
use strict;

my($ssc_fd);

sub ssc_open() {
#    my ($termios);
    $ssc_fd = POSIX::open('/dev/ttyS3', O_RDWR | O_NOCTTY) or die "$! in open";
#    $termios = POSIX::Termios->new;
#    $termios->getattr($ssc_fd) or die "$! in getattr";
#    print $termios->getispeed, " ", $termios->getospeed, " ", POSIX::B9600, " ", POSIX::B38400, "\n";
}

sub ssc_set_servo($$) {
    my ($servo, $position) = @_;

    print "ssc=$position\n";
    my $buff = chr(255) . chr($servo) . chr($position);
    POSIX::write($ssc_fd, $buff, 3) or die "$! in ssc_set_servo";
}

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 ssc_close {
    POSIX::close($ssc_fd);
}

sub 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", "/dev/ttyS2", "spd_vhi");
    $encoder_fd = POSIX::open('/dev/ttyS2', O_RDWR | O_NOCTTY | O_NONBLOCK) 
	or die "$! in open";
    cbreak($encoder_fd);
}

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

sub encoder_read() {
    my ($buff, $encoder_buff, $encoder_count, $ret, $speed);

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

    $encoder_count = unpack("l", $encoder_buff);
#      $speed = $encoder_count - $old_count;
#    print "$ret $encoder_count $speed\n";
#    $old_count = $encoder_count;
    return $encoder_count;
};

END {
    ssc_close();
    encoder_close();
};

#
# MAIN ROUTINE
#

ssc_open();
encoder_open();

my $current_output = 128;
my $max_output = 255;
my $min_output = 0;
my $new_output;

my $old_encoder_count = encoder_read();
my $encoder_count;
my ($current_speed, $desired_speed, $speed_error);

$desired_speed = shift (@ARGV);
my $K_pro = shift(@ARGV);
print "desired_speed = $desired_speed, K_pro = $K_pro\n";

for (;;) {
    select undef, undef, undef, 0.05;
    $encoder_count = encoder_read();
    $current_speed = 
	$encoder_count - 
	    $old_encoder_count;
    $old_encoder_count = $encoder_count;

    $speed_error = $desired_speed - $current_speed;
    print "err=$speed_error ";
    $new_output = $current_output + ($K_pro * $speed_error);
    $new_output = $max_output if $new_output > $max_output;
    $new_output = $min_output if $new_output < $min_output;
    ssc_set_servo(0, $new_output);
    $current_output = $new_output;
}