use strict;
use warnings;

use Math::Matrix;
use Math::MatrixLUP;
use Data::Roundtrip qw/perl2dump no-unicode-escape-permanently/;

# this is used for checking if two numbers are sufficiently
# close: abs(x-y)<SMALLNUMBER
# 1E-09 is too small! and for more dimensions 1E-05 is too small
my $SMALLNUMBER = 1E-02;

srand 125; # on the same page

my @planes3D = (
  [1, 2, 1, -1], # ax+by+cz+n=0
  [2, 3, -2, 2]
);

my $sol3D = intersection_by_solve(\@planes3D);
verifysolution(\@planes3D, $sol3D);
$sol3D = intersection_by_rref(\@planes3D);
verifysolution(\@planes3D, $sol3D);

my @planes5D = (
  [1, 2, 1, -2, 3, -1], # ax+by+cz+n=0
  [2, 3, -2, 4, -4, 2],
  [3, -1, -2, -5, 6, 2],
  [-3, 4, 2, 1, 1, 2]
);

my $sol5D = intersection_by_solve(\@planes5D);
verifysolution(\@planes5D, $sol5D);
$sol5D = intersection_by_rref(\@planes5D);
verifysolution(\@planes5D, $sol5D);

# this needs some time ...
#test_in_beast_space();

=pod

# edge cases, 1 dimension is all zeros
# this fails
@planes5D = (
  [0, 2, 1, -2, 3, -1], # ax+by+cz+n=0
  [0, 3, -2, 4, -4, 2],
  [0, -1, -2, -5, 6, 2],
  [0, 4, 2, 1, 1, 2]
);

$sol5D = intersection_by_solve(\@planes5D);
verifysolution(\@planes5D, $sol5D);

$sol5D = intersection_by_rref(\@planes5D);
print Math::MatrixLUP->new(\@planes5D)->rref;
verifysolution(\@planes5D, $sol5D);

# edge cases, 2 dimension is all zeros
# this fails
@planes5D = (
  [0, 0, 1, -2, 3, -1], # ax+by+cz+n=0
  [0, 0, -2, 4, -4, 2],
  [0, 0, -2, -5, 6, 2],
  [0, 0, 2, 1, 1, 2]
);

$sol5D = intersection_by_solve(\@planes5D);
verifysolution(\@planes5D, $sol5D);

$sol5D = intersection_by_rref(\@planes5D);
print Math::MatrixLUP->new(\@planes5D)->rref;
verifysolution(\@planes5D, $sol5D);
=cut

sub intersection_by_solve {
	my $planes = shift;
	die "at least two planes are parallel, can not continue."
	   if are_planes_parallel($planes) == 1;
	return Math::Matrix->new($planes)->solve->as_array;
}
sub intersection_by_rref {
	my $planes = shift;
	die "at least two planes are parallel, can not continue."
	   if are_planes_parallel($planes) == 1;

	my $sol = Math::MatrixLUP->new($planes)->rref->as_array;
	# leave only the last 2 cols
	return [ map { [ $_->[-2], $_->[-1] ] } @$sol ];
}

sub verifysolution {
	my ($planes, $sol) = @_;
	my $N = scalar(@{$planes->[0]}) - 1;
	#print perl2dump($sol, {terse=>1,indent=>1}). "verifying above solution in $N dims with ".scalar(@$planes)." planes ...\n";
	print "verifying solution in $N dims with ".scalar(@$planes)." planes ...\n";
	for (1..1000){
		my $t = -1000 + rand(2000);
		my @point;
		for my $i (0..$#$sol){
			push @point, -($sol->[$i][0] * $t) - $sol->[$i][1];
		}
		push @point, $t; # the last dimension

		# check if point is on all planes
		for my $plane (@$planes){
			my $res = 0.0;
			for my $i (0..$#point){
				$res += $point[$i] * $plane->[$i]
			}
			$res += $plane->[-1]; # the 'n'
			die "point (@point) not in plane (@$plane) ($res!=0) for t=$t"
			  unless abs($res) < $SMALLNUMBER;
		}
	}
	print "all points lie on the intersecting planes\n";
}

sub verifysolution_by_rref {
	my ($planes, $sol) = @_;
	my $N = scalar(@{$planes->[0]}) - 1;
	for (1..1000){
		my $t = -1000 + rand(2000);
		my @point;
		for my $i (0..$#$sol){
			push @point, -($sol->[$i][0] * $t) - $sol->[$i][1];
		}
		push @point, $t; # the last dimension

		# check if point is on all planes
		for my $plane (@$planes){
			my $res = 0.0;
			for my $i (0..$#point){
				$res += $point[$i] * $plane->[$i]
			}
			$res += $plane->[-1]; # the 'n'
			die "point (@point) not in plane (@$plane) ($res!=0) for t=$t"
			  unless abs($res) < $SMALLNUMBER;
		}
	}
	print "all points lie on the intersecting planes\n";
}

# Any number of planes, will be checked pairwise
# returns 1 if any two are parallel (and does not check for the rest)
# returns 0 if neither two are parallel (and does not check for the rest)
sub are_planes_parallel {
	my $planes = shift;
	my $N = scalar(@{ $planes->[0] }) - 1; # number of dimensions

	# test to see if planes are parallel, in which case there is no intersection:
	# taken pairwise, if the ratio of each of the coefficients (except the constant)
	# is the same then they are parallel
	# special care for zero coefficients, see below:
	for my $i (0..$#$planes){
		my $p1 = $planes->[$i];
		for my $j (($i+1)..$#$planes){
			my $p2 = $planes->[$j];
			my $ratio = undef;
			my $these_two_planes_are_parallel = 1;
			# we should not check the constant (n) in ax+by+cz+n
			INPLANES:
			for my $ii (0..($N-1)){
				# see https://perlmonks.org/?node_id=11160680
				# if both coefficients are zero then it is equivalent
				# to the ratio of these coefficients being the same as the previous ones
				# so we keep checking the other coefficients
				# because that implied planes are "parallel on that dimension"

				# two zeroes:
				# they are "parallel to this axis", continue with other coefficients
				next INPLANES if (abs($p1->[$ii]) < $SMALLNUMBER)
					      && (abs($p2->[$ii]) < $SMALLNUMBER)
				;

				# one or the other are zero : they are not parallel on this axis
				# and therefore the planes are not parallel,
				# no need to check other coefficients,
				# proceed with other pairs of planes
				if(  (abs($p1->[$ii]) < $SMALLNUMBER)
				 xor (abs($p2->[$ii]) < $SMALLNUMBER) # ^^ is in perl 5.40+
				){
					# because we complain if is undef below...
					$ratio = 0 unless defined $ratio;
					$these_two_planes_are_parallel = 0;
					last INPLANES
				}				
				unless(defined $ratio){
					# create a ratio if it was not already created
					# we will be checking all other ratios to this
					# if at least one is not the same (wrt range check below)
					# we break the loop because the planes are not parallel
					$ratio = $p1->[$ii] / $p2->[$ii];
					next INPLANES;
				}
				my $v = abs($ratio - $p1->[$ii] / $p2->[$ii]);
				if( abs($ratio - ($p1->[$ii] / $p2->[$ii])) > $SMALLNUMBER ){
					$these_two_planes_are_parallel = 0;
					last INPLANES
				}
			} # INPLANES : for each coefficient of the planes
			# no need to check other planes, at least two are parallel
			if( $these_two_planes_are_parallel > 0 ){
				return 1 # parallel!
			}
		} # end $j plane
	} # end $i plane
	return 0 # not parallel
}

sub test_in_beast_space {
	for (1..1){
		my $N = 666;
		my @planes = (undef) x ($N-1);
		for my $plane (@planes){
			#$plane = [ map { -10 + int rand(20) } (0..$N) ];
			$plane = [ map { -100 + rand(200) } (0..$N) ];
		}
		my $sol = intersection_by_solve(\@planes);
		verifysolution(\@planes, $sol);
		$sol = intersection_by_rref(\@planes);
		verifysolution(\@planes, $sol);
	}
	print "all points lie on the intersecting planes\n";
}