#!/usr/bin/env perl

# quick hack to read monitoring info from KLS-controllers (kellycontroller.com).
# maybe works for other controllers too, their KAC-series at least uses the same App.
# i guess you will smile about the extra-inefficient bit-operations below ;)

# by david schumacher, november 2017

use strict;
use Time::HiRes qw( usleep gettimeofday );
use Device::SerialPort;

my $DEBUG_RESPONSE = 0;
my $DEBUG_GENERAL = 0;

my ( @response_tmp_1, @response_tmp_2, @response_tmp_3, @response_complete, $i, %output, $key, $val ) = ( );

my @pos2name = init_pos2name( );

my $SERIAL_PORT_DEVICE = $ARGV[0];

my $serial_port = &open_serial_port( $SERIAL_PORT_DEVICE );

#+ startup:
# ** data sent: **
# 11 00 11
# F1 00 F1
# F1 00 F1

$| = 1;

while ( 1 ) {
	
	# send monitor-requests, read responses
	$serial_port->write( ord2string( 58, 0, 58 ) );
	@response_tmp_1 = string2ord ( read_serial_port() );

	$serial_port->write( ord2string( 59, 0, 59 ) );
	@response_tmp_2 = string2ord ( read_serial_port() );

	# if checksum does not match, reset port
	if ( checksum_ok( @response_tmp_1 ) < 1 || checksum_ok( @response_tmp_2 ) < 1 ) { # || checksum_ok( @response_tmp_3 ) < 1 ) {
		reset_serial_port('checksum failure');
	} else {
		# build one array out of the three responses
		# skip first (always contains the first byte of the monitor-request)
		# and last element (always contains already checked checksum)
		# of each array
		@response_complete = ( );
		for ( $i = 1; $i < $#response_tmp_1; $i ++ ) {
			push( @response_complete, $response_tmp_1[$i] );
		}
		
		for ( $i = 1; $i < $#response_tmp_2; $i ++ ) {
			push( @response_complete, $response_tmp_2[$i] );
		}

		%output = ();
		for ( $i = 0; $i <= $#response_complete; $i ++ ) {
			if ( defined ( $pos2name[$i] ) && $pos2name[$i] !~ /^_numbytes_$/ ) {
				if ( $pos2name[$i] !~ /^_/ ) {
					$output{ $pos2name[$i] } = $response_complete[$i];
				} elsif ( $pos2name[$i] =~ /^_2bytevalue_(\d+),(\d+)_(.*)_$/ ) {
					my ($pos_msb, $pos_lsb, $name) = ($1, $2, $3);
					
					if ( $i == 18 ) {
						# decode error status
						$output{ $name } = join( ', ', errorstate2text( twobytes2int( $response_complete[$pos_msb], $response_complete[$pos_lsb] ) ) );
					} else {
						$output{ "$name" } = twobytes2int( $response_complete[$pos_msb], $response_complete[$pos_lsb] );
					}
				}
			}
		}
	}

	$output{'Timestamp'} = int (gettimeofday * 1000);

	# do what you like with the output-hash
	foreach $key ( sort keys %output ) {
		print "$key : " . $output{ $key } . "\n";
	}
	
	print '-' x 80 . "\n";
	
	if ( $DEBUG_GENERAL > 0 ) {
		print STDERR "\n";
	}
}

sub open_serial_port {
	my ( $device ) = @_;
	my $serial_port = Device::SerialPort->new( $device ) or print "cant open serial port at $device: $!\n";
	$serial_port->baudrate( 19200 );
	$serial_port->databits( 8 );
	$serial_port->parity( 'none' );
	$serial_port->stopbits( 1 );
	$serial_port->write_settings;
	return $serial_port;
}

sub read_serial_port {
	my ( $count, $response ) = ( );
	
	# wait for controller to process/answer the request.
	# starting at around 22000 usec result is *sometimes* complete
	# starting at around 28000 usec result is *nearly allways* complete
	# so we added 4000 usecs to be sure
	# eval/alarm helps to prevent lockups if controller does not answer
	
	eval {
		local $SIG{ALRM} = sub { die "read timeout\n" };
		alarm 3;
	
		usleep( 32000 );
		while (1) {
			( $count, $response ) = $serial_port -> read( 10000 ); # will read _up to_ x chars
			if ( $count > 0 ) {
				last;
			}
		}
		alarm 0;
	};
	
	if ( $DEBUG_RESPONSE > 0 ) {
		dao( $response );
	}
	
	return $response;
}

sub reset_serial_port {
	my ($msg) = @_;
	print STDERR "$msg\n";
	print STDERR "resetting port...\n";
	
	$serial_port->close();
	undef $serial_port;
	$serial_port = &open_serial_port( $SERIAL_PORT_DEVICE );
}

# calculate checksum on controller response
sub checksum_calc {
	my ( @ord ) = @_;
	my ( $sum, $i ) = ( );

	# sum up array-elements without last element
	# last element contains checksum calculated by controller
	for ($i = 0; $i < $#ord; $i ++ ) {
		$sum += $ord[$i];
	}
	
	# modulo 256
	return ( $sum % 256 );
}

# check if checksum from controller matches own checksum
# match: 1 
# mismatch: 0
sub checksum_ok {
	my ( @ord ) = @_;
	my $checksum = checksum_calc( @ord );
	
	# last array element contains checksum calculated by controller
	if ( $checksum == $ord[$#ord] ) {
		return 1;
	} else {
		return 0;
	}
}

# values > 255 are represented by two bytes. MSB first, LSB second
# convert controller response to int
sub twobytes2int {
	my ( $msb, $lsb ) = @_;
	return int( ( $msb * 256 ) + $lsb );
}

sub errorstate2text {
	my @error_state_lookup = (
		"Identify Err",
		"Over Volt",
		"Low Volt",
		"Reserved",
		"Locking",
		"V+ Err",
		"Overtemp",
		"High Pedal",
		"Reserved",
		"Reset Error",
		"Pedal Error",
		"Hall Sensor Error",
		"Reserved",
		"Emergency Rev Err",
		"Motor OverTemp Err",
		"Current Meter Err"
	);

	my $bin = sprintf( '%016b', $_[0] );
	my $i = 0;
	my (@errortextlist, $bit) = ();
	
	foreach $bit ( reverse( split( //, $bin ) ) ) {
		if ( $bit == 1 ) {
			push( @errortextlist, $error_state_lookup[$i] );
		}
		$i ++;
	}
	return @errortextlist;
}

# Debug As Ord
sub dao {
	my ( $response ) = @_;
	my @tmp = string2ord( $response );
	foreach (@tmp) {
		print STDERR "$_ ";
	}
	
	if ( $#tmp > 0 ) {
		print STDERR "\n";
	}
}

sub string2ord {
	my ($string) = @_;
	my ($byte, @ord) = ();
	
	foreach $byte ( split(//, $string) ) {
		push ( @ord, sprintf( "%03d", ord($byte) ) );
	}
	return @ord;
}

sub ord2string {
	my (@ord) = @_;
	my $string = '';
	foreach (@ord) {
		$string .= chr($_);
	}
	return $string;
}

sub init_pos2name {

	return ( '_numbytes_',
		'TPS Pedal (0-255)',
		'Brake Pedal (0-255)',
		'Brake Switch (0|1)',
		'Foot Switch (0|1)',
		'Forward Switch (0|1)',
		'Reversed (0|1)',
		'Hall A (0|1)',
		'HaLL B (0|1)',
		'Hall C (0|1)',
		'B+ Volt (0-200V)',
		'Motor Temp (0-150C)',
		'Controller Temp (0-150C)',
		'Setting Dir (0|1, 0=F,1=R)',
		'Actual Dir (0|1, 0=F,1=R)',
		'Brake Switch2 (0|1)',
		'Low Speed (0|1)',
		'_numbytes_',
		'_2bytevalue_18,19_Error Status_',
		'_',
		'_2bytevalue_20,21_Motor Speed_',
		'_',
		'_2bytevalue_22,23_Phase Current_',
		'_' );
}

# aus android-app--decompilat:
#       strArr[0] = new String[]{"016", "2", "1", "h", "Error Status", "0", "65535", "1", "0", "错误状态,实时显示"};
#       strArr[1] = new String[]{"000", "1", "0", "uo", "TPS Pedel", "0", "255", "1", "0", "油门AD,Range 0~255 对应0~5V"};
#       strArr[2] = new String[]{"001", "1", "0", "uo", "Brake Pedel", "0", "255", "1", "0", "刹车AD,Range 0~255 对应0~5V"};
#       strArr[3] = new String[]{"002", "1", "0", "uo", "Brake Switch", "0", "2", "1", "0", "刹车开关状态,Range 0或1"};
#       strArr[4] = new String[]{"003", "1", "0", "uo", "Foot Switch", "0", "2", "1", "0", "油门安全开关状态,Range 0或1"};
#       strArr[5] = new String[]{"004", "1", "0", "uo", "Forward Switch", "0", "2", "1", "0", "前进开关状态,Range 0或1"};
#       strArr[6] = new String[]{"005", "1", "0", "uo", "Reversed", "0", "2", "1", "0", "后退开关状态,Range 0或1"};
#       strArr[7] = new String[]{"006", "1", "0", "uo", "Hall A", "0", "2", "1", "0", "HallA,编码器A,Range 0或1"};
#       strArr[8] = new String[]{"007", "1", "0", "uo", "Hall B", "0", "2", "1", "0", "HallB,编码器B,Range 0或1"};
#       strArr[9] = new String[]{"008", "1", "0", "uo", "Hall C", "0", "2", "1", "0", "HallC,Range 0或1"};
#       strArr[10] = new String[]{"009", "1", "0", "uo", "B+  Volt", "0", "200", "1", "0", "电池实际电压,Range 0~200V"};
#       strArr[11] = new String[]{"010", "1", "0", "uo", "Motor Temp", "0", "150", "1", "0", "电机温度,Range 0~150℃"};
#       strArr[12] = new String[]{"011", "1", "0", "uo", "Controller Temp", "0", "150", "1", "0", "控制器温度,Range 0~150℃"};
#       strArr[13] = new String[]{"012", "1", "0", "uo", "Setting Dir", "0", "2", "1", "0", "给定运行方向,0前进,1后退"};
#       strArr[14] = new String[]{"013", "1", "0", "uo", "Actual Dir", "0", "2", "1", "0", "实际运行方向,0前进,1后退"};
#       strArr[15] = new String[]{"014", "1", "0", "uo", "Brake Switch2", "0", "2", "1", "0", "刹车开关2状态,Range 0或1"};
#       
#       # letzer wert ohne offset: Low Speed (status wird auch dann im monitor angezeigt, wenn der controller nicht auf berücksichtigung des pins konfiguriert ist)
#       strArr[16] = new String[]{"015", "1", "0", "uo", "Low Speed", "0", "2", "1", "0", "刹车开关2状态,Range 0或1"}; 
#       
#       # motor speed: auf msbyte auf position 20, lsbyte auf position 21
#               strArr[17] = new String[]{"018", "2", "1", "uo", "Motor Speed", "0", "10000", "1", "0", "电机速度,Range 0~10000"};
#       
#       # phasenstrom: lsbyte auf position 23, msbyte auf position 22:
#       strArr[18] = new String[]{"020", "2", "1", "uo", "Phase Current", "0", "800", "1", "0", "相电流有效值,Range 0~800"};
#
#