#!/usr/bin/perl

use Math::Trig qw(deg2rad rad2deg pi);

## Parms:  matrix[], n
sub identity_matrix(\@$) {
	local (*mym) = shift(@_);
	my $n = shift(@_);

	my $sizen = $n * $n; 
	my $ii;
	for ( $ii=0; $ii<$sizen; $ii++ ) {
		$mym[$ii] = 0.0;
	}
	for ( $ii=0; $ii<$n; $ii++ ) {
		$mym[$n*$ii + $ii] = 1.0;
	}
}

 
## Parms:  matrix[], n, type['x', 'y' ó 'z'], angle
sub compound_matrix(\@$$$) {
	local (*mym) = shift(@_);
	my $n = shift(@_);
	my $type = shift(@_);
	my $ang = shift(@_);

	identity_matrix( @mym, $n );

	$ang = deg2rad( $ang );

	if( $type eq 'z' ) {
		$mym[0] = cos( $ang ); 
		$mym[1] = -sin( $ang ); 
		$mym[3] = -$mym[1];
		$mym[4] = $mym[0];
	}
	elsif ( $type eq 'y' ) {
        $mym[0] = cos( $ang );
        $mym[2] = sin( $ang );   
        $mym[6] = -$mym[2];
        $mym[8] = $mym[0];
    }
	elsif ( $type eq 'x' ) {
        $mym[4] = cos( $ang );
        $mym[5] = -sin( $ang );   
        $mym[7] = -$mym[5];
        $mym[8] = $mym[4];
    }
}

### vector = matrix * vector
sub matrix_x_vector(\@\@\@) {
	local (*v1) = shift(@_);
	local (*mymat) = shift(@_);
	local (*v2) = shift(@_);

	$v1[0] = $mymat[0] * $v2[0] + $mymat[1] * $v2[1] + $mymat[2] * $v2[2];
	$v1[1] = $mymat[3] * $v2[0] + $mymat[4] * $v2[1] + $mymat[5] * $v2[2];
	$v1[2] = $mymat[6] * $v2[0] + $mymat[7] * $v2[1] + $mymat[8] * $v2[2];
}

### matrix = matrix * matrix
sub matrix_x_matrix(\@\@\@) {
	local (*m1) = shift(@_);
	local (*m2) = shift(@_);
	local (*m3) = shift(@_);

	my $val;
	for( my $i=0; $i<9; $i++ ) {
		for( my $j=0; $j<3; $j++ ) {
			$val = 0.0;
			for( my $k=0; $k<3; $k++ ) {
				$val += $m2[3*$i + $k] * $m3[ 3*$k + $j];
			}
			$m1[ 3*$i + $j] = $val;
		}
	}
}

### point to point
sub distance(\@\@) {
	local (*n1) = shift(@_);
	local (*n2) = shift(@_);
	my $x = $n1[0] - $n2[0];
	my $y = $n1[1] - $n2[1];
	my $z = $n1[2] - $n2[2];

	return( sqrt( $x*$x + $y*$y + $z*$z ) );
}

sub voea( $$$$$ ) 
{
	my $the, $phi, $detthe;
	my  $i=0;

	my $theta1 = shift;
	my $theta2 = shift;
	my $phi1 = shift;
	my $phi2 = shift;
	my $delta = shift;

	if( ($theta1==0.0 && $theta2==0.0) || $theta1>=$theta2 ){
		$theta1 = 0.0;
		$theta2 = 359.9;
	}
	if( ($phi1==0.0 && $phi2==0.0) || $phi1>=$phi2) {
		$phi1 = 0.0;
		$phi2 = 90.0;
	}
	print("# Refinement program, Fraga 17/02/97\n");
	print("# Number  Theta  Phi   Psi\n");
	for( $phi=$phi1; $phi<=$phi2; $phi+=$delta ) {
		if( $phi==0.0 || $phi==180.0 ) {
			$detthe=360.0;
		}
		else {
			$detthe= $delta/sin( deg2rad( $phi ) );
		}

		my $sinphi = sin( deg2rad( $phi ) ); 
		my $cosphi = cos( deg2rad( $phi ) ); 

		for( $the=$theta1; $the<=$theta2; $the+=$detthe, $i++ ) {
			# printf( "%d %f %f\n", $i, $the, $phi );
			$thetas[ $i ] = $the;
			$phis[ $i ] = $phi;

			my $sintheta = sin( deg2rad( $the ) ); 
			my $costheta = cos( deg2rad( $the ) ); 
		 	$px[ $i ] = $sinphi * $costheta;
			$py[ $i ] = $sinphi * $sintheta;
			$pz[ $i ] = $cosphi;
		}
		$nelem = $i;
	}
}

sub do_normalization( $$ )
{
	my $i = shift;
	my $k = shift;
	my $j;

	compound_matrix( @Ty, 3, 'y', -$phis[$i]   );
	compound_matrix( @Tz, 3, 'z', -$thetas[$i] );
	matrix_x_matrix( @T, @Ty, @Tz);

	# Ok. We going to transform each point
 	for ( $j=0; $j<$k; $j++ ) {
		$index = $pointers[ $j ];

		$p1[0] = $px[ $index ];
		$p1[1] = $py[ $index ];
		$p1[2] = $pz[ $index ];

		matrix_x_vector( @p2, @T, @p1 );
		
		$px_n[ $j ] = $p2[0];
		$py_n[ $j ] = $p2[1];
		$pz_n[ $j ] = $p2[2];
	}
}

sub do_denormalization( $$ )
{
	my $i = shift;
	my $k = shift;
	my $j;

	compound_matrix( @Tz, 3, 'z', $thetas[$i] );
	compound_matrix( @Ty, 3, 'y', $phis[$i]   );
	matrix_x_matrix( @T, @Tz, @Ty);

	# Ok. We going to transform each point
 	for ( $j=0; $j<$k; $j++ ) {
 		$p1[ 0 ] = $xs[ $j ];
 		$p1[ 1 ] = $ys[ $j ];
 		$p1[ 2 ] = 1.0;

		matrix_x_vector( @p2, @T, @p1 );
		
		$px_n[ $j ] = $p2[0];
		$py_n[ $j ] = $p2[1];
		$pz_n[ $j ] = $p2[2];
	}
}
sub consistency( $ )
{
	my $k = shift;	
	my $j;
	# I have:
	# $pointers[]
	# $alpha[]
	# $d[]
	
 	for ( $j=0; $j<$k; $j++ ) {
		my $L1 = $d[ $j ]; 
		my $index = ($j + 1) % $k; 
		my $L2 = $d[ $index ]; 
	
		my $flag=0;  # $L1 < $L2
		if ($L2 < $L1 ) {
			$tmp = $L1;
			$L1 = $L2;
			$L2 = $tmp;
			$flag=1;
		}
		my $ang = abs( $alpha[ $index ] - $alpha[ $j ] );
		if ( $ang > 180 ) { $ang = 360 - $ang; }
		if ( $ang < 90.0 ) {
			my $l = $L1/cos( deg2rad( $ang ) );
			if ( $l < $L2 ) {
				# print "eles $L1 $L2 $ang\n";
				# Fault to consistency happens 
				if ( $flag ) {
					return $j;
				}
				else {
					return $index;
				}	
			}

		}
	}
	return -1;  # All it's ok
}

#Test..
# compound_matrix( @T, 3, 'z', 90.0   );
# $p1[0] = 1;
# $p1[1] = 0;
# $p1[2] = 0;
# print "$p1[0] $p1[1] $p1[2]\n";
# matrix_x_vector( @p2, @T, @p1 );
# print "$p2[0] $p2[1] $p2[2]\n";

# $p1[0] = 0;
# $p1[1] = 0;
# $p1[2] = 1;
# print "$p1[0] $p1[1] $p1[2]\n";
# compound_matrix( @T, 3, 'y', 90.0   );
# matrix_x_vector( @p2, @T, @p1 );
# print "$p2[0] $p2[1] $p2[2]\n";

die "Args: theta1 theta2 phi1 phi2 delta\n" if $#ARGV != 4;
$t1 = $ARGV[0];
$t2 = $ARGV[1]; 
$p1 = $ARGV[2];
$p2 = $ARGV[3];
$delta = $ARGV[4];

# $pi = pi;
# print "pi=$pi\n";

voea( $t1, $t2, $p1, $p2, $delta );

print "# Number of points = $nelem\n";

$p1[ 0 ] = $px[ 0 ];
$p1[ 1 ] = $py[ 0 ];
$p1[ 2 ] = $pz[ 0 ];

$p2[ 0 ] = $px[ 1 ];
$p2[ 1 ] = $py[ 1 ];
$p2[ 2 ] = $pz[ 1 ];
# if ( $delta < 15 ) {
# 	$ndelta = distance( @p1, @p2 );
# 	print "# distance = $ndelta\n";
# 	$ndelta *= 1.25;
# 	print "# distance = $ndelta\n";
# }
# else {
# 	$ndelta = distance( @p1, @p2 ) * 1.345; # 1.32 , 1.23
# }
$ndelta = distance( @p1, @p2 ) * 1.345; # 1.32 , 1.23

# STEP 1
# We get the nearest points to the current point
for ( $i=0; $i<$nelem; $i++ ) {
	$p1[ 0 ] = $px[ $i ];
	$p1[ 1 ] = $py[ $i ];
	$p1[ 2 ] = $pz[ $i ];
	LOOP2: for ( $j=0, $k=0; $j<$nelem; $j++ ) {
		next LOOP2 if $j == $i;
		
		$p2[ 0 ] = $px[ $j ];
		$p2[ 1 ] = $py[ $j ];
		$p2[ 2 ] = $pz[ $j ];

		$d = distance( @p1, @p2 );

		if ( $d < $ndelta ) {
			$ps[ $k ] = $j;
			$k ++;
		}
	}
	$points{ $i." ".0 } = $k;
	for ( $j=1; $j<=$k; $j++ ) {
		$points{ $i." ".$j } = $ps[ $j-1 ];
	}
}

# Testing...
# for ( $i=0; $i<$nelem; $i++ ) {
# 	print "i=$i ";
# 	$k = $points{ $i." ".0 };
# 	for ( $j=1; $j<=$k; $j++ ) {
# 		print "$points{ $i.\" \".$j } ";
# 	}
# 	print "\n";
# }

# STEP 2
# For every point and its neighbors :
#   We change to a "normalized" coordinates where
#   the current point must be on [0,0,1] 
#
for ( $i=0; $i<$nelem; $i++ ) {
 	$k = $points{ $i." ".0 };

 	for ( $j=1; $j<=$k; $j++ ) {
		$pointers[ $j - 1 ] = $points{ $i." ".$j };
	}

	do_normalization( $i, $k );

	# printf ( "%i %f %f %f\n", $i, l
	# We don't take account z, and we goind to calculate alphas
	# print "$i ";
 	for ( $j=0; $j<$k; $j++ ) {
		$alpha[ $j ] = rad2deg( atan2( $py_n[ $j ], $px_n[ $j ] ) );
		if ( $alpha[ $j ] < -1e-10 ) {
			$alpha[ $j ] += 360.0;
		}
		elsif ( abs( $alpha[ $j ] ) <= 1e-10 ) {
			$alpha[ $j ] = 0.0;
		}

		# print "$alpha[ $j ] ";
	}
	# print "\n";
	#
	# STEP 3
	# We need to sort the alphas' array
 	for ( $j=0; $j<$k; $j++ ) {
		$min = 361.0;
 		for ( $l = $j; $l<$k; $l++ ) {
			if( $alpha[ $l ] < $min ) {
				$min = $alpha[ $l ];
				$index = $l;
			}
		}
		# print "$index $min\n";
		if ( $index != $j ) {
			$tmp = $pointers[ $index ]; 
			$pointers[ $index ] = $pointers[ $j ]; 
			$pointers[ $j ] = $tmp;

			$tmp = $alpha[ $index ]; 
			$alpha[ $index ] = $alpha[ $j ];
			$alpha[ $j ] = $tmp;
		}
	}
 	for ( $j=1; $j<=$k; $j++ ) {
		$points{ $i." ".$j } = $pointers[ $j - 1 ];
		$tmp = $j - 1;
		$all_alphas{ $i." ".$tmp } = $alpha[ $tmp ];
	}

	# STEP 3.5 :-)
	# We need test each line among the others to check
	# consistency:
	#       Between two lines, the point where their
	#      bisectrixes instersects must be inside the
	#      the sector defined inside the two lines 

	# Now the points are ordered, but its better 
	# to recalculate their normalized coordinates
	
	do_normalization( $i, $k );

 	for ( $j=0; $j<$k; $j++ ) {
		$xok = $px_n[ $j ];
		$yok = $py_n[ $j ];

		$d[ $j ] = sqrt( $xok*$xok + $yok*$yok );
	}
	# Cheking consistency:
	$flag=1;
	# $n_cons=0;	
	do {
		$m = consistency ( $k );
		if ( $m == -1 ) { 
			$flag = 0; 
		}
		else {
			# $n_cons++;
			# print "m=$m\n";
			# erase item $m then
			splice( @pointers,$m,1 );
			splice( @alpha,$m,1 );
			splice( @d,$m,1 );
			# print "@pointers\n";
			$k--;
		}
	} while( $flag );
	# print "$i @pointers\n";

 	$points{ $i." ".0 } = $k;
 	for ( $j=1; $j<=$k; $j++ ) {
		$points{ $i." ".$j } = $pointers[ $j - 1 ];
		$tmp = $j - 1;
		$all_alphas{ $i." ".$tmp } = $alpha[ $tmp ];
	} 
	# End consistency
}

# Last STEPS !!! 
print "FACES $nelem\n";
for ( $i=0; $i<$nelem; $i++ ) {
	$k = $points{ $i." ".0 };

 	for ( $j=1; $j<=$k; $j++ ) {
		$pointers[ $j - 1 ] = $points{ $i." ".$j };
	}

	do_normalization( $i, $k );

	# Ok. I have now the central point (in $i )
	# and two vertices: $index1, $index2
	# We need calculate two perpendicular planes
	# (between $i and $index1, and $i and $index2)
	# their intersection (a line) and, finally,
	# twe calculate the point resultig of
	# intersect that line and the plane x-y.


	$index1 = 0;
	$p1[0] = $px_n[ $index1 ];
	$p1[1] = $py_n[ $index1 ];
	$p1[2] = $pz_n[ $index1 ];

	$e1[0] =  $p1[0]/2.0;
	$e1[1] =  $p1[1]/2.0;
	$e1[2] =  (1.0 + $p1[2])/2.0;

	$theta = $all_alphas{ $i." ".$index1 };
	# print "theta = $theta\n";
	$theta = deg2rad( $theta );
	$phi = atan2 ( 1.0-$p1[2], sqrt( $p1[0]*$p1[0] + $p1[1]*$p1[1] ) ); 

	# $kk = rad2deg( $phi );
	# print "phi = $kk\n";

	$ki = cos( $theta ) * sin( $phi ); 
	$kip = sin( $theta ) * sin( $phi ); 
	$kipp = cos( $phi ); 

	# print "DATA $i\n"; 
	print "# $i\n"; 
	printf ("NORMAL %1.7f %1.7f %1.7f\n", $px[$i], $py[$i], $pz[$i] ); 
	print "GREYTONE 250\n"; 
	print "POLYGON $k\n"; 

	$v = ( $e1[2] - 1.0 ) / $kipp;

 	for ( $j=1; $j<=$k; $j++ ) {
		$index2 = $j % $k;

		$p2[0] = $px_n[ $index2 ];
		$p2[1] = $py_n[ $index2 ];
		$p2[2] = $pz_n[ $index2 ];

		$e2[0] =  $p2[0]/2.0;
		$e2[1] =  $p2[1]/2.0;
		$e2[2] =  (1.0 + $p2[2])/2.0;

		$theta = $all_alphas{ $i." ".$index2 };
		# print "theta = $theta\n";
		$theta = deg2rad( $theta );
		$phi = atan2 ( 1.0-$p2[2], sqrt( $p2[0]*$p2[0] + $p2[1]*$p2[1] ) ); 

		# $kk = rad2deg( $phi );
		# print "phi = $kk\n";

		$kj = cos( $theta ) * sin( $phi ); 
		$kjp = sin( $theta ) * sin( $phi ); 
		$kjpp = cos( $phi ); 

		$t = ( $e2[2] - 1.0 ) / $kjpp;

		$c1 = $e2[0] - $e1[0] + $v*$ki - $t*$kj; 

		$c2 = $e2[1] - $e1[1] + $v*$kip - $t*$kjp; 


		$delta =  $p1[1] * $p2[0] - $p1[0] * $p2[1]; 
		$deltau = -$c1 * $p2[0] - $c2 * $p2[1]; 
		# $deltas = -$c2 * $p1[1] - $c1 * $p1[0];

		$u = $deltau / $delta;
		# $s = $deltas / $delta;

		$xok = $e1[0] - $u * $p1[1] - $v * $ki;
		# if ( abs ( $xok ) < 1e-10 ) { $xok = 0.0; } 
		$yok = $e1[1] + $u * $p1[0] - $v * $kip;
		# if ( abs ( $yok ) < 1e-10 ) { $yok = 0.0; } 
		# print "1 $xok $yok\n";

		$xs[ $j - 1 ] = $xok; 
		$ys[ $j - 1 ] = $yok; 

		# $xok = $e2[0] - $s * $p2[1] - $t * $kj;
		# # if ( abs ( $xok ) < 1e-10 ) { $xok = 0.0; } 
		# $yok = $e2[1] + $s * $p2[0] - $t * $kjp;
		# if ( abs ( $yok ) < 1e-10 ) { $yok = 0.0; } 
		# print "2 $xok $yok\n";

		$p1[0] = $p2[0]; $p1[1] = $p2[1]; $p1[2] = $p2[2]; 
		$e1[0] = $e2[0]; $e1[1] = $e2[1]; $e1[2] = $e2[2];

		$ki   = $kj;
		$kip  = $kjp;
		$kipp = $kjpp;

		$v = $t;
	}
	do_denormalization( $i, $k ); 
	
 	for ( $j=0; $j<$k; $j++ ) {
		printf ("%1.7f %1.7f %1.7f\n", $px_n[$j], $py_n[$j], $pz_n[$j] );
	}
}

die "# Ok!\n";

# Testing...
for ( $i=0; $i<$nelem; $i++ ) {
	print "i=$i ";
	$k = $points{ $i." ".0 };
	for ( $j=1; $j<=$k; $j++ ) {
		print "$points{ $i.\" \".$j } ";
	}
	print "\n";
}