#!/usr/bin/perl -w
# regex.pl
# test some regex value
# 01/03/2012
use strict;
use warnings;
use Math::Trig;
use File::Basename;  # split path ($name,$dir,$ext) = fileparse($file [, qr/\.[^.]*/] )
use Cwd;
my $os = $^O;
my $perl_dir = '/home/geoff/bin';
my $PATH_SEP = '/';
my $temp_dir = '/tmp';
if ($os =~ /win/i) {
    $perl_dir = 'C:\GTools\perl';
    $temp_dir = $perl_dir;
    $PATH_SEP = "\\";
}
unshift(@INC, $perl_dir);
require 'lib_utils.pl' or die "Unable to load 'lib_utils.pl' Check paths in \@INC...\n";
# log file stuff
our ($LF);
my $pgmname = $0;
if ($pgmname =~ /(\\|\/)/) {
    my @tmpsp = split(/(\\|\/)/,$pgmname);
    $pgmname = $tmpsp[-1];
}
my $outfile = $temp_dir.$PATH_SEP."temp.$pgmname.txt";
open_log($outfile);

my @decimals = qw( 123.12 2 56754 9292929292992.12 0.21 3.1 -123456.789123456 );

#my $test1 = 'BR-NVS@LOCAL: 322514.895268 5645174.336093 2947179.140007 27.687425 86.730184 9193.939175 -4.002387 -0.934358 0.737291 Aircraft/Embraer-195/Models/Embraer-195.xml';
#my $test2 = 'Keith@mpserver05: -2705820.907254 -4274373.134043 3871439.972421 37.612155 -122.335176 41.160700 -2.169488 -0.454383 -0.867152 Aircraft/ZLT-NT/Models/ZLT-NT.xml';
#my $test3 = 'lukasz@85: -2645158.289384 -4294376.158765 3891056.513310 37.835549 -121.631353 65.838404 -1.762055 1.411790 0.113173 Aircraft/Lockheed1049h/Models/Lockheed1049h.xml';
my $test4 = 'Hub ert@LOCAL: -2707426.292678 -4273107.700880 3871894.294430 37.616553 -122.358205 404.827472 0.903545 2.442853 1.213190 Aircraft/bluebird/Model';

#my $test = 'BR-N:   322514.895268 5645174.336093  2947179.140007   27.687425  86.730184 9193.939175 -4.002387 -0.934358 0.737291 Aircraft/Embraer-195/Models/Embraer-195.xml';
#my $test1 = 'GA02@RELAY: -4634470.057641 2825522.151699 -3339048.352732 -31.770420 148.630368 1449.370967 1.175368 -2.391665 3.037587 c172p';
#my $test2 = 'GA02@RELAY: -4634472.533845 2825517.492964 -3339048.839365 -31.770425 148.630423 1449.344449 1.172507 -2.382572 3.038499 c172p';
#my $test3 = 'GA02@RELAY: -4634474.453641 2825513.916359 -3339049.198367 -31.770429 148.630466 1449.343568 1.170331 -2.375576 3.039239 c172p';

my $test1 = 'GA02@RELAY: -4634127.780293 2826321.351137 -3338855.500296 -31.768353 148.621284 1461.701452 0.571001 -4.278450 2.484639 c172p';
my $test2 = 'GA02@RELAY: -4634126.206779 2826317.911353 -3338860.056284 -31.768403 148.621306 1460.828131 0.586041 -4.264497 2.495898 c172p';
my $test3 = 'GA02@RELAY: -4634125.064672 2826315.365341 -3338863.389862 -31.768440 148.621323 1460.168755 0.596528 -4.254179 2.503971 c172p';

my @strings = ($test1,$test2,$test3,$test4);

my $load_log = 1;

sub pgm_exit($$) {
    my ($val,$msg) = @_;
    if (length($msg)) {
        $msg .= "\n" if (!($msg =~ /\n$/));
        prt($msg);
    }
    ## //show_warnings($val);
    close_log($outfile,$load_log);
    exit($val);
}

# ============================================================ #
# SimGear Services, rendered in perl
# ============================================================ #
my $SGD_PI = 3.1415926535;
my $SGD_DEGREES_TO_RADIANS = $SGD_PI / 180.0;
my $SGD_RADIANS_TO_DEGREES = 180.0 / $SGD_PI;
my $SGD_MIN = 1.17549e-038;

#/// Quaternion
# x,y,z,w
my $QX = 0;
my $QY = 1;
my $QZ = 2;
my $QW = 3;
#/// Vector(3)
# x,y,z
my $VX = 0;
my $VY = 1;
my $VZ = 2;

sub fgs_rad2deg($) {
    my ($rad) = @_;
    return ($rad * $SGD_RADIANS_TO_DEGREES);
}

# dot(const SGVec3<T>& v1, const SGVec3<T>& v2)
# { return v1(0)*v2(0) + v1(1)*v2(1) + v1(2)*v2(2); }
# Given 2 Vectors3, return the dot product
sub scalar_dot_product($$) {
    my ($rv1,$rv2) = @_;
    return ${$rv1}[0] * ${$rv2}[0] + ${$rv1}[1] * ${$rv2}[1] + ${$rv1}[2] * ${$rv2}[2];
}

# The euclidean norm of the vector, that is what most people call length
# norm(const SGVec3<T>& v)
# { return sqrt(dot(v, v)); }
# Given a Vector3, return length
sub norm_vector_length($) {
    my ($rv) = @_;
    return sqrt(scalar_dot_product($rv, $rv));
}

sub get_quat_stg($) {
    my ($rv4) = @_;
    my $x = ${$rv4}[$QX];
    my $y = ${$rv4}[$QY];
    my $z = ${$rv4}[$QZ];
    my $w = ${$rv4}[$QW];
    return sprintf("$x $y $z $w");
}

sub get_vec3_stg($) {
    my ($rv3) = @_;
    my $x = ${$rv3}[0];
    my $y = ${$rv3}[1];
    my $z = ${$rv3}[2];
    return sprintf("$x $y $z");
}


# print out a quaternion - x,y,z,w
sub show_quat($) {
    my ($rv4) = @_;
    my $x = ${$rv4}[$QX];
    my $y = ${$rv4}[$QY];
    my $z = ${$rv4}[$QZ];
    my $w = ${$rv4}[$QW];
    print "x $x, y $y, z $z, w $w\n";
}

# print out a vector3
sub show_vec3($) {
    my ($rv3) = @_;
    my $x = ${$rv3}[0];
    my $y = ${$rv3}[1];
    my $z = ${$rv3}[2];
    print "x $x, y $y, z $z\n";
}

#/// The conjugate of the quaternion, this is also the
#/// inverse for normalized quaternions
#SGQuat<T> conj(const SGQuat<T>& v)
#{ return SGQuat<T>(-v(0), -v(1), -v(2), v(3)); }
sub quat_conj($) {
    my ($rq) = @_;
    my @q = (0,0,0,0);
    $q[$QX] = -${$rq}[$QX];
    $q[$QY] = -${$rq}[$QY];
    $q[$QZ] = -${$rq}[$QZ];
    $q[$QW] =  ${$rq}[$QW];
    # return [ -${$rq}[0], -${$rq}[1], -${$rq}[2], ${$rq}[3] ];
    return \@q;
}


#/// Quaternion multiplication
sub mult_quats($$) {
    my ($rv1,$rv2) = @_;
    my @v = (0,0,0,0);
    $v[$QX] = ${$rv1}[$QW] * ${$rv2}[$QX] + ${$rv1}[$QX] * ${$rv2}[$QW] + ${$rv1}[$QY] * ${$rv2}[$QZ] - ${$rv1}[$QZ] * ${$rv2}[$QY];
    $v[$QY] = ${$rv1}[$QW] * ${$rv2}[$QY] - ${$rv1}[$QX] * ${$rv2}[$QZ] + ${$rv1}[$QY] * ${$rv2}[$QW] + ${$rv1}[$QZ] * ${$rv2}[$QX];
    $v[$QZ] = ${$rv1}[$QW] * ${$rv2}[$QZ] + ${$rv1}[$QX] * ${$rv2}[$QY] - ${$rv1}[$QY] * ${$rv2}[$QX] + ${$rv1}[$QZ] * ${$rv2}[$QW];
    $v[$QW] = ${$rv1}[$QW] * ${$rv2}[$QW] - ${$rv1}[$QX] * ${$rv2}[$QX] - ${$rv1}[$QY] * ${$rv2}[$QY] - ${$rv1}[$QZ] * ${$rv2}[$QZ];
    return \@v;
}

#SGVec3<T> mult(const SGVec3<T>& v1, const SGVec3<T>& v2)
#{ return SGVec3<T>(v1(0)*v2(0), v1(1)*v2(1), v1(2)*v2(2)); }
sub mult_vec3($$) {
    my ($rv1,$rv2) = @_;
    my @v = (0,0,0);
    $v[0] = ${$rv1}[0] * ${$rv2}[0];
    $v[1] = ${$rv1}[1] * ${$rv2}[1];
    $v[2] = ${$rv1}[2] * ${$rv2}[2];
    return \@v;
}

#/// Scalar multiplication
#template<typename S, typename T>
# SGVec3<T> operator*(S s, const SGVec3<T>& v)
#{ return SGVec3<T>(s*v(0), s*v(1), s*v(2)); }
sub scalar_mult_vector($$) {
    my ($s,$rv) = @_;
    my @v = (0,0,0);
    $v[0] = ${$rv}[0] * $s;
    $v[1] = ${$rv}[1] * $s;
    $v[2] = ${$rv}[2] * $s;
    my $txt = sprintf("Mult Point3D %s, by %f, to get %s",
        get_vec3_stg($rv), $s, get_vec3_stg(\@v));
    prt("$txt\n");
    return \@v;
}

#  /// write the euler angles into the references
#  void getEulerRad(T& zRad, T& yRad, T& xRad) const {
#    T sqrQW = w()*w();
#    T sqrQX = x()*x();
#    T sqrQY = y()*y();
#    T sqrQZ = z()*z();
#    T num = 2*(y()*z() + w()*x());
#    T den = sqrQW - sqrQX - sqrQY + sqrQZ;
#    if (fabs(den) <= SGLimits<T>::min() &&
#        fabs(num) <= SGLimits<T>::min())
#      xRad = 0;
#    else
#      xRad = atan2(num, den);
#    T tmp = 2*(x()*z() - w()*y());
#    if (tmp <= -1)
#      yRad = T(0.5)*SGMisc<T>::pi();
#    else if (1 <= tmp)
#      yRad = -T(0.5)*SGMisc<T>::pi();
#    else
#      yRad = -asin(tmp);
#    num = 2*(x()*y() + w()*z()); 
#    den = sqrQW + sqrQX - sqrQY - sqrQZ;
#    if (fabs(den) <= SGLimits<T>::min() &&
#        fabs(num) <= SGLimits<T>::min())
#      zRad = 0;
#    else {
#      T psi = atan2(num, den);
#      if (psi < 0)
#        psi += 2*SGMisc<T>::pi();
#      zRad = psi;
#    }
#  }
sub getEulerRad($$$$) {
    my ($rq, $rzRad, $ryRad, $rxRad) = @_;
    my ($xRad,$yRad,$zRad);
    my $sqrQW = ${$rq}[$QW] * ${$rq}[$QW];
    my $sqrQX = ${$rq}[$QX] * ${$rq}[$QX];
    my $sqrQY = ${$rq}[$QY] * ${$rq}[$QY];
    my $sqrQZ = ${$rq}[$QZ] * ${$rq}[$QZ];

    # y * z + w * x
    my $num = 2 * ( ${$rq}[$QY] * ${$rq}[$QZ] + ${$rq}[$QW] * ${$rq}[$QX] );
    my $den = $sqrQW - $sqrQX - $sqrQY + $sqrQZ;
    if ((abs($den) <= 0.0000001) &&
        (abs($num) <= 0.0000001) ) {
        $xRad = 0;
    } else {
        $xRad = atan2($num, $den);
    }

    # x * z - w * y
    my $tmp = 2 * ( ${$rq}[$QX] * ${$rq}[$QZ] - ${$rq}[$QW] * ${$rq}[$QY] );
    if ($tmp <= -1) {
        $yRad = 0.5 * $SGD_PI;
    } elsif (1 <= $tmp) {
        $yRad = - 0.5 * $SGD_PI;
    } else {
        $yRad = -asin($tmp); # needs Math::Trig
    }

    # x * y + w * z
    $num = 2 * ( ${$rq}[$QX] * ${$rq}[$QY] + ${$rq}[$QW] * ${$rq}[$QZ] ); 
    $den = $sqrQW + $sqrQX - $sqrQY - $sqrQZ;
    if ((abs($den) <= 0.0000001) &&
        (abs($num) <= 0.0000001) ) {
        $zRad = 0;
    } else {
        my $psi = atan2($num, $den);
        if ($psi < 0) {
            $psi += 2 * $SGD_PI;
        }
        $zRad = $psi;
    }
    # pass value back
    ${$rxRad} = $xRad;
    ${$ryRad} = $yRad;
    ${$rzRad} = $zRad;
}

# uses getEulerRad, and converts to degrees
sub getEulerDeg($$$$) {
    my ($rq,$rzDeg,$ryDeg,$rxDeg) = @_;
    my ($xRad,$yRad,$zRad);
    getEulerRad($rq, \$zRad, \$yRad, \$xRad);
    # pass converted values back
    ${$rzDeg} = fgs_rad2deg($zRad);
    ${$ryDeg} = fgs_rad2deg($yRad);
    ${$rxDeg} = fgs_rad2deg($xRad);
}

#  static SGQuat fromRealImag(T r, const SGVec3<T>& i) {
#    SGQuat q;
#    q.w() = r;
#    q.x() = i.x();
#    q.y() = i.y();
#    q.z() = i.z();
#    return q; }
sub fromRealImag($$) {
    my ($r, $ri) = @_;
    my @q = (0,0,0,0);
    $q[$QW] = $r;
    $q[$QX] = ${$ri}[0];
    $q[$QY] = ${$ri}[1];
    $q[$QZ] = ${$ri}[2];
    my $txt = sprintf("fromRealImag: r=%f Point3D %s, Quat %s", $r, get_vec3_stg($ri), get_quat_stg(\@q));
    prt("$txt\n");

    return \@q;
}

#  /// Create a quaternion from the angle axis representation where the angle
#  /// is stored in the axis' length
#  static SGQuat fromAngleAxis(const SGVec3<T>& axis) {
#    T nAxis = norm(axis);
#    if (nAxis <= SGLimits<T>::min())
#      return SGQuat::unit();
#    T angle2 = T(0.5)*nAxis;
#    return fromRealImag(cos(angle2), T(sin(angle2)/nAxis)*axis); }
sub fromAngleAxis($) {
    my ($raxis) = @_;
    my $nAxis = norm_vector_length($raxis);
    if ($nAxis <= 0.0000001) {
        my @arr = (0,0,0,0);
        return \@arr; # SGQuat::unit();
    }
    my $angle2 = $nAxis * 0.5;
    my $sang = sin($angle2) / $nAxis ;
    my $cang = cos($angle2);
    #print "nAxis = $nAxis, ange2 = $angle2, saxa = $sang\n";
    my $txt = sprintf("fromAngleAxis: p3d %s, gave nAxis=%f, angle2=%f, sang=%f, cang=%f",
        get_vec3_stg($raxis), $nAxis, $angle2, $sang, $cang);
    prt("$txt\n");
    my $rv = scalar_mult_vector($sang,$raxis);
    #print "san ";
    #show_vec3($rv);
    #return fromRealImag(cos(angle2), T(sin(angle2)/nAxis)*axis);
    return fromRealImag( $cang, $rv );
}

#  /// Return a quaternion rotation from the earth centered to the
#  /// simulation usual horizontal local frame from given
#  /// longitude and latitude.
#  /// The horizontal local frame used in simulations is the frame with x-axis
#  /// pointing north, the y-axis pointing eastwards and the z axis
#  /// pointing downwards.
#  static SGQuat fromLonLatRad(T lon, T lat)
#    SGQuat q;
#    T zd2 = T(0.5)*lon;
#    T yd2 = T(-0.25)*SGMisc<T>::pi() - T(0.5)*lat;
#    T Szd2 = sin(zd2);
#    T Syd2 = sin(yd2);
#    T Czd2 = cos(zd2);
#    T Cyd2 = cos(yd2);
#    q.w() = Czd2*Cyd2;
#    q.x() = -Szd2*Syd2;
#    q.y() = Czd2*Syd2;
#    q.z() = Szd2*Cyd2;
#    return q;  }
sub fromLonLatRad($$) {
    my ($lonr,$latr) = @_;
    my @q = (0,0,0,0);
    my $zd2 = 0.5 * $lonr; 
    my $yd2 = -0.25 * $SGD_PI - (0.5 * $latr);
    my $Szd2 = sin($zd2);
    my $Syd2 = sin($yd2);
    my $Czd2 = cos($zd2);
    my $Cyd2 = cos($yd2);
    $q[$QW] = $Czd2 * $Cyd2;
    $q[$QX] = - $Szd2 * $Syd2;
    $q[$QY] = $Czd2 * $Syd2;
    $q[$QZ] = $Szd2 * $Cyd2;
    return \@q;
}

#void euler_get(float lat, float lon, float ox, float oy, float oz,
#    float *head, float *pitch, float *roll)
#{
#    /* FGMultiplayMgr::ProcessPosMsg */
#    SGVec3f angleAxis;
#    angleAxis(0) = ox;
#    angleAxis(1) = oy;
#    angleAxis(2) = oz;
#    SGQuatf ecOrient;
#    ecOrient = SGQuatf::fromAngleAxis(angleAxis);
#    /* FGAIMultiplayer::update */
#    float lat_rad, lon_rad;
#    lat_rad = lat * SGD_DEGREES_TO_RADIANS;
#    lon_rad = lon * SGD_DEGREES_TO_RADIANS;
#    SGQuatf qEc2Hl = SGQuatf::fromLonLatRad(lon_rad, lat_rad);
#    SGQuatf hlOr = conj(qEc2Hl) * ecOrient;
#    float hDeg, pDeg, rDeg;
#    hlOr.getEulerDeg(hDeg, pDeg, rDeg);
#    if(head)
#        *head = hDeg;
#    if(pitch)
#        *pitch = pDeg;
#    if(roll)
#        *roll = rDeg;
#}
sub euler_get($$$$$$$$) {
    my ($lat, $lon, $ox, $oy, $oz, $rhead, $rpitch, $rroll) = @_;
    #/* FGMultiplayMgr::ProcessPosMsg */
    my @angleAxis = ($ox,$oy,$oz);
    my ($txt);
    #push(@angleAxis, $ox);
    #push(@angleAxis, $oy);
    #push(@angleAxis, $oz);
    #print "angleAxis ";
    #show_vec3(\@angleAxis);
    my $recOrient = fromAngleAxis(\@angleAxis); # ecOrient = SGQuatf::fromAngleAxis(angleAxis);
    $txt = sprintf("From Point3D %s, got sgdQuat %s",
        get_vec3_stg(\@angleAxis), get_quat_stg($recOrient));
    prt("$txt\n");
    #print "ecOrient ";
    #show_quat($recOrient);
    #/* FGAIMultiplayer::update */
    my ($lat_rad, $lon_rad);
    $lat_rad = $lat * $SGD_DEGREES_TO_RADIANS;
    $lon_rad = $lon * $SGD_DEGREES_TO_RADIANS;
    my $qEc2Hl = fromLonLatRad($lon_rad, $lat_rad);
    $txt = sprintf("From lat/lon %f,%f, rad %f,%f, fromLonLatRad %s",
        $lat, $lon, $lat_rad, $lon_rad, get_quat_stg($qEc2Hl));
    prt("$txt\n");
    #print "fromLonLatRad ";
    #show_quat($qEc2Hl);
    my $con = quat_conj($qEc2Hl);
    #print "conj ";
    #show_quat($con);
    my $rhlOr = mult_quats($con, $recOrient);
    $txt = sprintf("From quat_conj %s, from mult_quats %s\n",
        get_quat_stg($con), get_quat_stg($rhlOr));
    prt("$txt\n");
    #print "mult ";
    #show_quat($rhlOr);
    getEulerDeg($rhlOr, $rhead, $rpitch, $rroll );
    $txt = sprintf("getEulerDeg returned h=%f, p=%f, r=%f\n",
        ${$rhead}, ${$rpitch}, ${$rroll});
    prt("$txt\n");

}

# ================================================================ #
# End SimGear Fuctions
# ================================================================ #



sub test_decimal() {
    my ($val,$msg);
    foreach $val (@decimals) {
        $msg = "NOT decimal";
        if ($val =~ /^[+-]?\d+(\.\d+)?$/) {
            $msg = 'is decimal';
        }
        prt("Value: [$val] $msg\n");
    }
}

sub escape_strs($) {
    my ($str) = @_;
    $str =~ s/"/\&quot;/g;
    $str =~ s/'/\&apos;/g;
    $str =~ s/\&/\&amp;/g;
    $str =~ s/>/\&gt;/g;
    $str =~ s/</\&lt;/g;
    return $str;
}


sub test_strings() {
    my ($stg);
    my ($callsign,$server,$val1,$val2,$val3,$val4,$val5,$val6,$val7,$val8,$val9,$model);
    foreach $stg (@strings) {
        prt("sg $stg\n");
        #if ($stg =~ /^([\w-\s]+)@(.+):{1}(\s+[+-]?\d+\.\d+){9}\s+(.+)$/) {
        #if ($stg =~ /^([\w-]+)@(.+):{1}(\s+[+-]?\d+(\.\d+)?)\s+(.+)$/) {
        #if ($stg =~ /^([\w-]+)@(.+):{1}(\s+[+-]?\d+(\.\d+)?){9}\s+(.+)$/) {
        #              CS      HOST        Dec1                Dec2                 Dec3                 Dec4                 Dec5                 Dec6                 Dec7                 Dec8                 Dec9                 Model         
        #if ($stg =~ /^([\w-]+)@(.+):{1}\s+([+-]?\d+(\.\d+)?)\s+([+-]?\d+(\.\d+)?)\s+([+-]?\d+(\.\d+)?)\s+([+-]?\d+(\.\d+)?)\s+([+-]?\d+(\.\d+)?)\s+([+-]?\d+(\.\d+)?)\s+([+-]?\d+(\.\d+)?)\s+([+-]?\d+(\.\d+)?)\s+([+-]?\d+(\.\d+)?)\s+(.+)$/) {
        if ($stg =~ /^([-\w\s]+)@(.+):{1}\s+([+-]?\d+\.\d+)\s+([+-]?\d+\.\d+)\s+([+-]?\d+\.\d+)\s+([+-]?\d+\.\d+)\s+([+-]?\d+\.\d+)\s+([+-]?\d+\.\d+)\s+([+-]?\d+\.\d+)\s+([+-]?\d+\.\d+)\s+([+-]?\d+\.\d+)\s+(.+)$/) {
            $callsign = $1;
            $server = $2;
            $val1 = $3;
            $val2 = $4;
            $val3 = $5;
            $val4 = $6;
            $val5 = $7;
            $val6 = $8;
            $val7 = $9;
            $val8 = $10;
            $val9 = $11;
            $model = $12;
            #prt("ok $callsign,$server,1-$val1,2-$val2,3-$val3,4-$val4,5-$val5,6-$val6,7-$val7,8-$val8,9-$val9,m-$model\n");
            #my ($head,$roll,$pitch);
            #prt("ok [$callsign];[$server];[$val1];[$val2];[$model]\n");
        } else {
            prt("failed $stg");
        }
        # or FROM Pigeon's script
        if($stg =~ m/^(.*)@(.*?): (-?[0-9\.]+) (-?[0-9\.]+) (-?[0-9\.]+) (-?[0-9\.]+) (-?[0-9\.]+) (-?[0-9\.]+) (-?[0-9\.]+) (-?[0-9\.]+) (-?[0-9\.]+) (.*?)$/) {
            my ($callsign, $server_ip,
                        $x, $y, $z,
                        $lat, $lon, $alt,
                        $ox, $oy, $oz,
                        $model) =
                    ($1, $2, $3, $4, $5, $6, $7, $8, $9, $10, $11, $12);
            my ($head,$pitch,$roll);
            prt("ok = $callsign\@$server_ip $x $y $z $lat $lon $alt $ox $oy $oz $model\n");
            prt("euler_get($lat, $lon, $ox, $oy, $oz, ...\n");
            euler_get($lat, $lon, $ox, $oy, $oz, \$head, \$pitch, \$roll);
            prt("Pilot: [$callsign], at lat,lon,alt $lat,$lon,$alt,\n orentation head,pitch,roll $head,$pitch,$roll\n");
        } else {
            prt("regex FAILED on $stg\n");
        }
    }
}


#test_decimal();

test_strings();

pgm_exit(0,"");
