#!/usr/bin/perl -w

use Math::Trig;
use ASKxml;

#

sub elem2array
{
  my $e = $_[0];
  return ($e->{x}, $e->{y}, $e->{z});
}

sub array2elem
{
  return { x=>$_[0], y=>$_[1], z=>$_[2] };
}

sub NormaVektoru
{
  my @v = @_;
  return my $delta = sqrt($v[0]*$v[0] + $v[1]*$v[1] + $v[2]*$v[2]);
};

sub DisplacedMidPoint
{
  my @A = elem2array($_[0]);
  my @Z = elem2array($_[1]);
  my $limit = $_[2];
# vektor usecky AZ
  my @dvec = map { $Z[$_] - $A[$_] } (0..2);
# delka usecky AZ
  my $delAZ = NormaVektoru(@dvec);
#  print("delAZ = $delAZ\n");
  if ($delAZ < $limit) {
    return undef;
  }
  my $delAZpul = $delAZ/2;
# bod v prostredku usecky AZ
  my @su =  map { $A[$_] + $dvec[$_]/2 } (0..2);

#
# volime nahodne bod "m1" na kruznici o polomeru "er" se stredem v bode "su"
# v rovine kolme na primku AZ
# tim z primky AZ udelame dve primky AM1 a M1Z
#
  my $fi = deg2rad(rand(359));
#
#
  my $er = $delAZpul;
  my @m1 = ($er * cos($fi), $er * sin($fi), 0);

#
# urcime jednotkovy vektor "q" usecky AZ
#
  my @q = map { $dvec[$_]/$delAZ } (0..2);

#
# jednotkovy vektor "z" kolmy na rovinu, ve ktere lezi bod "m1"
#
  my @z = (0,0,1); 

#
# uhel "theta" mezi vektory "q" a "z"
#
  my $theta = acos( $q[2] );

#
# vektor "u s carkou" je vektorovy soucin "q" a "z"
#
  my @uprim = (-$q[1], $q[0], 0);

#
# tento vektor je nutne znormovat
#
  my $deluprim = NormaVektoru(@uprim);

#
# oprava na situaci, kdy jsou vektory "q" a "z" temer rovnobezne
#
  my @u = $deluprim > 0.00001 ? map { $uprim[$_]/$deluprim } (0..2) : (1, 0, 0);

#
# matice rotace
#
  my $cosTh = cos($theta);
  my $sinTh = sin($theta);
  my $JmcosTh = 1 - $cosTh;
  my $u0JmcosTh = $u[0]*$JmcosTh;
  my $u1JmcosTh = $u[1]*$JmcosTh;

  my @RotX = ( $cosTh + $u[0]*$u0JmcosTh,       $u[1]*$u0JmcosTh - $u[2]*$sinTh, $u[2]*$u0JmcosTh + $u[1]*$sinTh );
  my @RotY = ( $u[1]*$u0JmcosTh + $u[2]*$sinTh, $cosTh +  $u[1]*$u1JmcosTh,      $u[2]*$u1JmcosTh - $u[0]*$sinTh );
  my @RotZ = ( $u[2]*$u0JmcosTh - $u[1]*$sinTh, $u[2]*$u1JmcosTh + $u[0]*$sinTh, $cosTh +  $u[2]*$u[2]*$JmcosTh );

#
# rotace bodu "m1" o uhel "theta", jde o soucin matice rotace o uhel "theta" a "m1"
#
  my @mprim = ( $RotX[0]*$m1[0] + $RotX[1]*$m1[1] + $RotX[2]*$m1[2],
                $RotY[0]*$m1[0] + $RotY[1]*$m1[1] + $RotY[2]*$m1[2],
                $RotZ[0]*$m1[0] + $RotZ[1]*$m1[1] + $RotZ[2]*$m1[2]);

#
# hledany bod M, k rotaci pricteme translaci
#
  my @M = map { $mprim[$_] + $su[$_] } (0..2);

#
# zaokrouhleni osizeno funkci int  (chyba muze byt maximalne 20 cm v realu)
#
  @M = map { int($M[$_]) } (0..2);

#
# vytvoreni vystupniho seznamu tvoreneho body A, M, Z
#
  #my $p = {};
  #push(@{$p->{bod}}, array2elem(@A));
  #push(@{$p->{bod}}, array2elem(@M));
  #push(@{$p->{bod}}, array2elem(@Z));
  #return $p;
  return array2elem(@M);
};

sub iterator
{
  my $list = shift;
  my $i = 0;
  return sub {
    return if $i > $#$list;
    return $list->[$i++];
  }
};

# -------------------------------------------------------------------------
#
# main
#
# -------------------------------------------------------------------------

my $proudIN  = $ARGV[0];
my $proudOUT = $ARGV[1];
my $limit = 2;
#print "nacitam: $proudIN\n";

my $proud = ASKxmlin("$proudIN") || die;

my @body_in = @{$proud->{bod}};
my @body_out = ();

my $a = shift(@body_in);

while (@body_in) {
  my $b = shift(@body_in);
  print "a: $a->{x}, $a->{y}, $a->{z} \n";
  print "b: $b->{x}, $b->{y}, $b->{z}  \n";

  my $m = DisplacedMidPoint($a, $b, $limit);

  if (defined($m)) {
    print "m: $m->{x}, $m->{y}, $m->{z}  \n";
    unshift(@body_in, $m, $b);
  }
  else {
    push(@body_out, $a);
    $a = $b;
  }
  print "\n";
}

push(@body_out, $a) if (defined($a));

#

$proud = { body=>\@body_out };
print ASKxmlout($proud, rootname=>"proud");
ASKxmlout($proud, rootname=>"proud", outputfile=>"VodaProudOUT");

##

##