#!/usr/bin/env perl
# 20260416 ChatGPT
# $Header$
#
# Example:
#   perl imu_roll_pitch_demo.pl
#   perl imu_roll_pitch_demo.pl sideways
#   perl imu_roll_pitch_demo.pl upright
#
# Notes:
#   * Computes roll and pitch from accelerometer values.
#   * Uses:
#       roll  = atan2(Az, Ay)
#       pitch = atan2(-Ax, sqrt(Ay^2 + Az^2))
#   * Angles are printed in both radians and degrees.
#

use strict;
use warnings;
use POSIX qw(atan2);

my %cases = (
    sideways => {
        Ax => -0.951,
        Ay => -0.043,
        Az => -0.003,
    },
    upright => {
        Ax =>  0.028,
        Ay =>  0.987,
        Az =>  0.012,
    },
);

my @requested_cases;
if (@ARGV) {
    for my $name (@ARGV) {
        if (!exists $cases{$name}) {
            die "Unknown case '$name'. Valid cases: sideways upright\n";
        }
        push @requested_cases, $name;
    }
}
else {
    @requested_cases = qw(sideways upright);
}

for my $name (@requested_cases) {
    my $ax = $cases{$name}->{Ax};
    my $ay = $cases{$name}->{Ay};
    my $az = $cases{$name}->{Az};

    my $roll_rad  = atan2($az, $ay);
    my $pitch_rad = atan2(-$ax, sqrt($ay * $ay + $az * $az));

    my $roll_deg  = rad_to_deg($roll_rad);
    my $pitch_deg = rad_to_deg($pitch_rad);

    printf "%s\n", ucfirst($name);
    printf "  A: X=% .3f  Y=% .3f  Z=% .3f\n", $ax, $ay, $az;
    printf "  roll  (rad) = % .6f\n", $roll_rad;
    printf "  roll  (deg) = % .3f\n", $roll_deg;
    printf "  pitch (rad) = % .6f\n", $pitch_rad;
    printf "  pitch (deg) = % .3f\n", $pitch_deg;
    print "\n";
}

exit 0;

sub rad_to_deg {
    my ($rad) = @_;
    return $rad * 180.0 / pi();
}

sub pi {
    return 4.0 * atan2(1.0, 1.0);
}