#!/usr/bin/perl -w
use strict;
use warnings;
use Data::Dumper;
use feature qw/ say /;

BEGIN {
    use constant PI => 3.14159265358979;

    sub deg2rad {
        my $degrees = shift;
        return ($degrees / 180) * PI;
    }

    sub rad2deg {
        my $radians = shift;
        return ($radians / PI) * 180;
    }
}

my $num_args=$#ARGV+1;
if($num_args < 3) {
        print "Usage: combine_doc.pl <doc_file_1> <doc_file_2> <combined_doc>\n";
        exit;
}

chomp(my $indoc_file1_name=$ARGV[0]);
chomp(my $indoc_file2_name=$ARGV[1]);
chomp(my $outdoc_file_name=$ARGV[2]);

open INDOC1,"<$indoc_file1_name" || die "Can't open file: $!";

my $line=0;
my @data;
my @data_doc1;
my $i=0;

foreach (<INDOC1>) {
        if($line==0) {
                $line++;
        }
        elsif($line%2!=0) {
                @data=split(" ",$_);
                $data_doc1[$i]=$data[1];
                $i++;
                $line++;
        }
        else {
                @data=split(" ",$_);
                $data_doc1[$i]=$data[2];
                $data_doc1[$i+1]=$data[3];
                $data_doc1[$i+2]=$data[4];
                $data_doc1[$i+3]=$data[5];
                $data_doc1[$i+4]=$data[6];
                $data_doc1[$i+5]=$data[7];
                $i+=6;
                $line++;
        }
}
print scalar(@data_doc1);
print "\n";
#print "@data_doc1";
#print "\n";

open INDOC2,"<$indoc_file2_name" || die "Can't open file: $!";

$line=0;
my @data_doc2;
$i=0;

foreach (<INDOC2>) {
        if($line==0) {
                $line++;
        }
        elsif($line%2!=0) {
                @data=split(" ",$_);
                $data_doc2[$i]=$data[1];
                $i++;
                $line++;
        }
        else {
                @data=split(" ",$_);
                $data_doc2[$i]=$data[2];
                $data_doc2[$i+1]=$data[3];
                $data_doc2[$i+2]=$data[4];
                $data_doc2[$i+3]=$data[5];
                $data_doc2[$i+4]=$data[6];
                $data_doc2[$i+5]=$data[7];
                $i+=6;
                $line++;
        }
}
#print scalar(@data_doc2);
#print "\n";
#print "@data_doc2";
#print "\n";

open OUTDOC,">$outdoc_file_name" || die "Can't open file: $!";
print OUTDOC " ; Headerinfo columns: rot (1), tilt (2), psi (3), Xoff (4), Yoff (5), Zoff (6), Ref (7), Wedge (8), Pmax/sumP (9), LL (10)\n";
my $rot_mat1;
my $rot_mat2;

my $part_id;
my $cbn_mat;
my @cbn_angles;
my @cbn_shifts;
for($i=1;$i<scalar(@data_doc1);$i+=7) {
        #print "$i $data_doc1[$i] $data_doc1[$i+1] $data_doc1[$i+2]\n";
        #print "$i $data_doc2[$i] $data_doc2[$i+1] $data_doc2[$i+2]\n";
        $rot_mat1=eul2mat($data_doc1[$i],$data_doc1[$i+1],$data_doc1[$i+2]);  #Catch the reference to the first matrix 
        $rot_mat2=eul2mat($data_doc2[$i],$data_doc2[$i+1],$data_doc2[$i+2]);  #Catch the reference to the second matrix
        $cbn_mat=mat_multiply($rot_mat1,$rot_mat2); #Multiply the two matrices
        @cbn_angles=mat2eul($cbn_mat); #Convert the matrix back to euler
        print "@cbn_angles";
        print "\n";
        $cbn_shifts[0]=$data_doc1[$i+3]+$data_doc2[$i+3];
        $cbn_shifts[1]=$data_doc1[$i+4]+$data_doc2[$i+4];
        $cbn_shifts[2]=$data_doc1[$i+5]+$data_doc2[$i+5];
        print OUTDOC " ; $data_doc1[$i-1]\n";
        $part_id=(($i-1)/7)+1;
        printf OUTDOC "%6d%3d%11.5f%11.5f%11.5f%11.5f%11.5f%11.5f%11.5f%11.5f%11.5f%11.5f\n",$part_id,10,$cbn_angles[0],$cbn_angles[1],$cbn_angles[2],$cbn_shifts[0],$cbn_shifts[1],$cbn_shifts[2],1,1,0,0;
}

sub eul2mat {
        my $phi_rad=deg2rad($_[0]);
        my $theta_rad=deg2rad($_[1]);
        my $psi_rad=deg2rad($_[2]);
        #print "$phi_rad $theta_rad $psi_rad\n";
        my @rot_mat;
        $rot_mat[0][0]=cos($phi_rad)*cos($theta_rad)*cos($psi_rad)-sin($phi_rad)*sin($psi_rad);
        $rot_mat[0][1]=cos($phi_rad)*cos($theta_rad)*sin($psi_rad)+sin($phi_rad)*cos($psi_rad);
        $rot_mat[0][2]=-cos($phi_rad)*sin($theta_rad);
        $rot_mat[1][0]=-sin($phi_rad)*cos($theta_rad)*cos($psi_rad)-sin($psi_rad)*cos($phi_rad);
        $rot_mat[1][1]=-sin($phi_rad)*cos($theta_rad)*sin($psi_rad)+cos($phi_rad)*cos($psi_rad);
        $rot_mat[1][2]=sin($phi_rad)*sin($theta_rad);
        $rot_mat[2][0]=sin($theta_rad)*cos($psi_rad);
        $rot_mat[2][1]=sin($theta_rad)*sin($psi_rad);
        $rot_mat[2][2]=cos($theta_rad);
        #say Dumper \@rot_mat;
        return \@rot_mat;
}

sub mat_multiply {
        my ($mat1,$mat2)=@_;
        my ($i,$j,$k);
        my $result_mat=[];
        for $i (0..2) {
                for $j (0..2) {
                        for $k (0..2) {
                                $result_mat->[$i][$j]+=$mat1->[$i][$k] * $mat2->[$k][$j];
                        }
                }
        }
        return $result_mat;
}

sub mat2eul {
        my @cbn_mat=@{$_[0]};
        my $theta_deg=rad2deg(atan2((sqrt($cbn_mat[2][0]**2+$cbn_mat[2][1]**2)),$cbn_mat[2][2]));
        my $phi_deg=rad2deg(atan2(($cbn_mat[1][2]/sin(deg2rad($theta_deg))),(-$cbn_mat[0][2]/sin(deg2rad($theta_deg)))));
        my $psi_deg=rad2deg(atan2(($cbn_mat[2][1]/sin(deg2rad($theta_deg))),($cbn_mat[2][0]/sin(deg2rad($theta_deg)))));
        return ($theta_deg,$phi_deg,$psi_deg);
}