// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
// Government sponsorship acknowledged. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0

#pragma once

#include <stdbool.h>

// Unless specified all arrays stored in contiguous matrices in row-major order.
//
// All functions are defined using the mrcal_..._full() form, which supports
// non-contiguous input and output arrays, and some optional arguments. Strides
// are used to specify the array layout.
//
// All functions have a convenience wrapper macro that is a simpler way to call
// the function, usable with contiguous arrays and defaults.
//
// All the functions use double-precision floating point to store the data, and
// C ints to store strides. The strides are given in bytes. In the
// mrcal_..._full() functions, each array is followed by the strides, one per
// dimension.
//
// I have two different representations of pose transformations:
//
// - Rt is a concatenated (4,3) array: Rt = nps.glue(R,t, axis=-2). The
//   transformation is R*x+t
//
// - rt is a concatenated (6,) array: rt = nps.glue(r,t, axis=-1). The
//   transformation is R*x+t where R = R_from_r(r)
//
// I treat all vectors as column vectors, so matrix multiplication works from
// the left: to rotate a vector x by a rotation matrix R I have
//
//   x_rotated = R * x

// Store an identity rotation matrix into the given (3,3) array
//
// This is simply an identity matrix
#define mrcal_identity_R(R) mrcal_identity_R_full(R, 0, 0)
void mrcal_identity_R_full(
    double* R,      // (3,3) array
    int R_stride0,  // in bytes. <= 0 means "contiguous"
    int R_stride1   // in bytes. <= 0 means "contiguous"
);

// Store an identity rodrigues rotation into the given (3,) array
//
// This is simply an array of zeros
#define mrcal_identity_r(r) mrcal_identity_r_full(r, 0)
void mrcal_identity_r_full(
    double* r,     // (3,) array
    int r_stride0  // in bytes. <= 0 means "contiguous"
);

// Store an identity Rt transformation into the given (4,3) array
#define mrcal_identity_Rt(Rt) mrcal_identity_Rt_full(Rt, 0, 0)
void mrcal_identity_Rt_full(
    double* Rt,      // (4,3) array
    int Rt_stride0,  // in bytes. <= 0 means "contiguous"
    int Rt_stride1   // in bytes. <= 0 means "contiguous"
);

// Store an identity rt transformation into the given (6,) array
#define mrcal_identity_rt(rt) mrcal_identity_rt_full(rt, 0)
void mrcal_identity_rt_full(
    double* rt,     // (6,) array
    int rt_stride0  // in bytes. <= 0 means "contiguous"
);

// Rotate the point x_in in a (3,) array by the rotation matrix R in a (3,3)
// array. This is simply the matrix-vector multiplication R x_in
//
// The result is returned in a (3,) array x_out.
//
// The gradient dx_out/dR is returned in a (3, 3,3) array J_R. Set to NULL if
// this is not wanted
//
// The gradient dx_out/dx_in is returned in a (3,3) array J_x. This is simply
// the matrix R. Set to NULL if this is not wanted
//
// In-place operation is supported; the output array may be the same as the
// input arrays to overwrite the input.
#define mrcal_rotate_point_R(x_out, J_R, J_x, R, x_in) \
    mrcal_rotate_point_R_full(                         \
        x_out,                                         \
        0,                                             \
        J_R,                                           \
        0,                                             \
        0,                                             \
        0,                                             \
        J_x,                                           \
        0,                                             \
        0,                                             \
        R,                                             \
        0,                                             \
        0,                                             \
        x_in,                                          \
        0,                                             \
        false                                          \
    )
#define mrcal_rotate_point_R_inverted(x_out, J_R, J_x, R, x_in) \
    mrcal_rotate_point_R_full(                                  \
        x_out,                                                  \
        0,                                                      \
        J_R,                                                    \
        0,                                                      \
        0,                                                      \
        0,                                                      \
        J_x,                                                    \
        0,                                                      \
        0,                                                      \
        R,                                                      \
        0,                                                      \
        0,                                                      \
        x_in,                                                   \
        0,                                                      \
        true                                                    \
    )
void mrcal_rotate_point_R_full(  // output
    double* x_out,               // (3,) array
    int x_out_stride0,           // in bytes. <= 0 means "contiguous"
    double* J_R,                 // (3,3,3) array. May be NULL
    int J_R_stride0,             // in bytes. <= 0 means "contiguous"
    int J_R_stride1,             // in bytes. <= 0 means "contiguous"
    int J_R_stride2,             // in bytes. <= 0 means "contiguous"
    double* J_x,                 // (3,3) array. May be NULL
    int J_x_stride0,             // in bytes. <= 0 means "contiguous"
    int J_x_stride1,             // in bytes. <= 0 means "contiguous"

    // input
    const double* R,     // (3,3) array. May be NULL
    int R_stride0,       // in bytes. <= 0 means "contiguous"
    int R_stride1,       // in bytes. <= 0 means "contiguous"
    const double* x_in,  // (3,) array. May be NULL
    int x_in_stride0,    // in bytes. <= 0 means "contiguous"

    bool inverted  // if true, I apply a
                   // rotation in the opposite
                   // direction. J_R corresponds
                   // to the input R
);

// Rotate the point x_in in a (3,) array by the rodrigues rotation in a (3,)
// array.
//
// The result is returned in a (3,) array x_out.
//
// The gradient dx_out/dr is returned in a (3,3) array J_r. Set to NULL if this
// is not wanted
//
// The gradient dx_out/dx_in is returned in a (3,3) array J_x. Set to NULL if
// this is not wanted
//
// In-place operation is supported; the output array may be the same as the
// input arrays to overwrite the input.
#define mrcal_rotate_point_r(x_out, J_r, J_x, r, x_in) \
    mrcal_rotate_point_r_full(                         \
        x_out,                                         \
        0,                                             \
        J_r,                                           \
        0,                                             \
        0,                                             \
        J_x,                                           \
        0,                                             \
        0,                                             \
        r,                                             \
        0,                                             \
        x_in,                                          \
        0,                                             \
        false                                          \
    )
#define mrcal_rotate_point_r_inverted(x_out, J_r, J_x, r, x_in) \
    mrcal_rotate_point_r_full(                                  \
        x_out,                                                  \
        0,                                                      \
        J_r,                                                    \
        0,                                                      \
        0,                                                      \
        J_x,                                                    \
        0,                                                      \
        0,                                                      \
        r,                                                      \
        0,                                                      \
        x_in,                                                   \
        0,                                                      \
        true                                                    \
    )
void mrcal_rotate_point_r_full(  // output
    double* x_out,               // (3,) array
    int x_out_stride0,           // in bytes. <= 0 means "contiguous"
    double* J_r,                 // (3,3) array. May be NULL
    int J_r_stride0,             // in bytes. <= 0 means "contiguous"
    int J_r_stride1,             // in bytes. <= 0 means "contiguous"
    double* J_x,                 // (3,3) array. May be NULL
    int J_x_stride0,             // in bytes. <= 0 means "contiguous"
    int J_x_stride1,             // in bytes. <= 0 means "contiguous"

    // input
    const double* r,     // (3,) array. May be NULL
    int r_stride0,       // in bytes. <= 0 means "contiguous"
    const double* x_in,  // (3,) array. May be NULL
    int x_in_stride0,    // in bytes. <= 0 means "contiguous"

    bool inverted  // if true, I apply a
                   // rotation in the opposite
                   // direction. J_r corresponds
                   // to the input r
);

// Transform the point x_in in a (3,) array by the Rt transformation in a (4,3)
// array.
//
// The result is returned in a (3,) array x_out.
//
// The gradient dx_out/dRt is returned in a (3, 4,3) array J_Rt. Set to NULL if
// this is not wanted
//
// The gradient dx_out/dx_in is returned in a (3,3) array J_x. This is simply
// the matrix R. Set to NULL if this is not wanted
//
// In-place operation is supported; the output array may be the same as the
// input arrays to overwrite the input.
#define mrcal_transform_point_Rt(x_out, J_Rt, J_x, Rt, x_in) \
    mrcal_transform_point_Rt_full(                           \
        x_out,                                               \
        0,                                                   \
        J_Rt,                                                \
        0,                                                   \
        0,                                                   \
        0,                                                   \
        J_x,                                                 \
        0,                                                   \
        0,                                                   \
        Rt,                                                  \
        0,                                                   \
        0,                                                   \
        x_in,                                                \
        0,                                                   \
        false                                                \
    )
#define mrcal_transform_point_Rt_inverted(x_out, J_Rt, J_x, Rt, x_in) \
    mrcal_transform_point_Rt_full(                                    \
        x_out,                                                        \
        0,                                                            \
        J_Rt,                                                         \
        0,                                                            \
        0,                                                            \
        0,                                                            \
        J_x,                                                          \
        0,                                                            \
        0,                                                            \
        Rt,                                                           \
        0,                                                            \
        0,                                                            \
        x_in,                                                         \
        0,                                                            \
        true                                                          \
    )
void mrcal_transform_point_Rt_full(  // output
    double* x_out,                   // (3,) array
    int x_out_stride0,               // in bytes. <= 0 means "contiguous"
    double* J_Rt,                    // (3,4,3) array. May be NULL
    int J_Rt_stride0,                // in bytes. <= 0 means "contiguous"
    int J_Rt_stride1,                // in bytes. <= 0 means "contiguous"
    int J_Rt_stride2,                // in bytes. <= 0 means "contiguous"
    double* J_x,                     // (3,3) array. May be NULL
    int J_x_stride0,                 // in bytes. <= 0 means "contiguous"
    int J_x_stride1,                 // in bytes. <= 0 means "contiguous"

    // input
    const double* Rt,    // (4,3) array. May be NULL
    int Rt_stride0,      // in bytes. <= 0 means "contiguous"
    int Rt_stride1,      // in bytes. <= 0 means "contiguous"
    const double* x_in,  // (3,) array. May be NULL
    int x_in_stride0,    // in bytes. <= 0 means "contiguous"

    bool inverted  // if true, I apply a
                   // transformation in the opposite
                   // direction. J_Rt corresponds
                   // to the input Rt
);

// Transform the point x_in in a (3,) array by the rt transformation in a (6,)
// array.
//
// The result is returned in a (3,) array x_out.
//
// The gradient dx_out/drt is returned in a (3,6) array J_rt. Set to NULL if
// this is not wanted
//
// The gradient dx_out/dx_in is returned in a (3,3) array J_x. This is simply
// the matrix R. Set to NULL if this is not wanted
//
// In-place operation is supported; the output array may be the same as the
// input arrays to overwrite the input.
#define mrcal_transform_point_rt(x_out, J_rt, J_x, rt, x_in) \
    mrcal_transform_point_rt_full(                           \
        x_out,                                               \
        0,                                                   \
        J_rt,                                                \
        0,                                                   \
        0,                                                   \
        J_x,                                                 \
        0,                                                   \
        0,                                                   \
        rt,                                                  \
        0,                                                   \
        x_in,                                                \
        0,                                                   \
        false                                                \
    )
#define mrcal_transform_point_rt_inverted(x_out, J_rt, J_x, rt, x_in) \
    mrcal_transform_point_rt_full(                                    \
        x_out,                                                        \
        0,                                                            \
        J_rt,                                                         \
        0,                                                            \
        0,                                                            \
        J_x,                                                          \
        0,                                                            \
        0,                                                            \
        rt,                                                           \
        0,                                                            \
        x_in,                                                         \
        0,                                                            \
        true                                                          \
    )
void mrcal_transform_point_rt_full(  // output
    double* x_out,                   // (3,) array
    int x_out_stride0,               // in bytes. <= 0 means "contiguous"
    double* J_rt,                    // (3,6) array. May be NULL
    int J_rt_stride0,                // in bytes. <= 0 means "contiguous"
    int J_rt_stride1,                // in bytes. <= 0 means "contiguous"
    double* J_x,                     // (3,3) array. May be NULL
    int J_x_stride0,                 // in bytes. <= 0 means "contiguous"
    int J_x_stride1,                 // in bytes. <= 0 means "contiguous"

    // input
    const double* rt,    // (6,) array. May be NULL
    int rt_stride0,      // in bytes. <= 0 means "contiguous"
    const double* x_in,  // (3,) array. May be NULL
    int x_in_stride0,    // in bytes. <= 0 means "contiguous"

    bool inverted  // if true, I apply the
                   // transformation in the
                   // opposite direction.
                   // J_rt corresponds to
                   // the input rt
);

// Convert a rotation matrix in a (3,3) array to a rodrigues vector in a (3,)
// array
//
// The result is returned in a (3,) array r
//
// The gradient dr/dR is returned in a (3, 3,3) array J. Set to NULL if this is
// not wanted
#define mrcal_r_from_R(r, J, R) mrcal_r_from_R_full(r, 0, J, 0, 0, 0, R, 0, 0)
void mrcal_r_from_R_full(  // output
    double* r,             // (3,) vector
    int r_stride0,         // in bytes. <= 0 means "contiguous"
    double* J,             // (3,3,3) array. Gradient. May be NULL
    int J_stride0,         // in bytes. <= 0 means "contiguous"
    int J_stride1,         // in bytes. <= 0 means "contiguous"
    int J_stride2,         // in bytes. <= 0 means "contiguous"

    // input
    const double* R,  // (3,3) array
    int R_stride0,    // in bytes. <= 0 means "contiguous"
    int R_stride1     // in bytes. <= 0 means "contiguous"
);

// Convert a rodrigues vector in a (3,) array to a rotation matrix in a (3,3)
// array
//
// The result is returned in a (3,3) array R
//
// The gradient dR/dr is returned in a (3,3 ,3) array J. Set to NULL if this is
// not wanted
#define mrcal_R_from_r(R, J, r) mrcal_R_from_r_full(R, 0, 0, J, 0, 0, 0, r, 0)
void mrcal_R_from_r_full(  // outputs
    double* R,             // (3,3) array
    int R_stride0,         // in bytes. <= 0 means "contiguous"
    int R_stride1,         // in bytes. <= 0 means "contiguous"
    double* J,             // (3,3,3) array. Gradient. May be NULL
    int J_stride0,         // in bytes. <= 0 means "contiguous"
    int J_stride1,         // in bytes. <= 0 means "contiguous"
    int J_stride2,         // in bytes. <= 0 means "contiguous"

    // input
    const double* r,  // (3,) vector
    int r_stride0     // in bytes. <= 0 means "contiguous"
);

// Invert a rotation matrix. This is a transpose
//
// The input is given in R_in in a (3,3) array
//
// The result is returned in a (3,3) array R_out
//
// In-place operation is supported; the output array may be the same as the
// input arrays to overwrite the input.
#define mrcal_invert_R(R_out, R_in) mrcal_invert_R_full(R_out, 0, 0, R_in, 0, 0)
void mrcal_invert_R_full(  // output
    double* R_out,         // (3,3) array
    int R_out_stride0,     // in bytes. <= 0 means "contiguous"
    int R_out_stride1,     // in bytes. <= 0 means "contiguous"

    // input
    const double* R_in,  // (3,3) array
    int R_in_stride0,    // in bytes. <= 0 means "contiguous"
    int R_in_stride1     // in bytes. <= 0 means "contiguous"
);

// Convert an Rt transformation in a (4,3) array to an rt transformation in a
// (6,) array
//
// The result is returned in a (6,) array rt
//
// The gradient dr/dR is returned in a (3, 3,3) array J_R. Set to NULL if this
// is not wanted
//
// The t terms are identical, so dt/dt = identity and I do not return it
//
// The r and R terms are independent of the t terms, so dr/dt and dt/dR are both
// 0, and I do not return them
#define mrcal_rt_from_Rt(rt, J_R, Rt) \
    mrcal_rt_from_Rt_full(rt, 0, J_R, 0, 0, 0, Rt, 0, 0)
void mrcal_rt_from_Rt_full(  // output
    double* rt,              // (6,) vector
    int rt_stride0,          // in bytes. <= 0 means "contiguous"
    double* J_R,             // (3,3,3) array. Gradient. May be NULL
    // No J_t. It's always the identity
    int J_R_stride0,  // in bytes. <= 0 means "contiguous"
    int J_R_stride1,  // in bytes. <= 0 means "contiguous"
    int J_R_stride2,  // in bytes. <= 0 means "contiguous"

    // input
    const double* Rt,  // (4,3) array
    int Rt_stride0,    // in bytes. <= 0 means "contiguous"
    int Rt_stride1     // in bytes. <= 0 means "contiguous"
);

// Convert an rt transformation in a (6,) array to an Rt transformation in a
// (4,3) array
//
// The result is returned in a (4,3) array Rt
//
// The gradient dR/dr is returned in a (3,3 ,3) array J_r. Set to NULL if this
// is not wanted
//
// The t terms are identical, so dt/dt = identity and I do not return it
//
// The r and R terms are independent of the t terms, so dR/dt and dt/dr are both
// 0, and I do not return them
#define mrcal_Rt_from_rt(Rt, J_r, rt) \
    mrcal_Rt_from_rt_full(Rt, 0, 0, J_r, 0, 0, 0, rt, 0)
void mrcal_Rt_from_rt_full(  // output
    double* Rt,              // (4,3) array
    int Rt_stride0,          // in bytes. <= 0 means "contiguous"
    int Rt_stride1,          // in bytes. <= 0 means "contiguous"
    double* J_r,             // (3,3,3) array. Gradient. May be NULL
    // No J_t. It's just the identity
    int J_r_stride0,  // in bytes. <= 0 means "contiguous"
    int J_r_stride1,  // in bytes. <= 0 means "contiguous"
    int J_r_stride2,  // in bytes. <= 0 means "contiguous"

    // input
    const double* rt,  // (6,) vector
    int rt_stride0     // in bytes. <= 0 means "contiguous"
);

// Invert an Rt transformation
//
// The input is given in Rt_in in a (4,3) array
//
// The result is returned in a (4,3) array Rt_out
//
// In-place operation is supported; the output array may be the same as the
// input arrays to overwrite the input.
#define mrcal_invert_Rt(Rt_out, Rt_in) \
    mrcal_invert_Rt_full(Rt_out, 0, 0, Rt_in, 0, 0)
void mrcal_invert_Rt_full(  // output
    double* Rt_out,         // (4,3) array
    int Rt_out_stride0,     // in bytes. <= 0 means "contiguous"
    int Rt_out_stride1,     // in bytes. <= 0 means "contiguous"

    // input
    const double* Rt_in,  // (4,3) array
    int Rt_in_stride0,    // in bytes. <= 0 means "contiguous"
    int Rt_in_stride1     // in bytes. <= 0 means "contiguous"
);

// Invert an rt transformation
//
// The input is given in rt_in in a (6,) array
//
// The result is returned in a (6,) array rt_out
//
// The gradient dtout/drin is returned in a (3,3) array dtout_drin. Set to NULL
// if this is not wanted
//
// The gradient dtout/dtin is returned in a (3,3) array dtout_dtin. Set to NULL
// if this is not wanted
//
// The gradient drout/drin is always -identity. So it is not returned
//
// The gradient drout/dtin is always 0. So it is not returned
//
// In-place operation is supported; the output array may be the same as the
// input arrays to overwrite the input.
#define mrcal_invert_rt(rt_out, dtout_drin, dtout_dtin, rt_in) \
    mrcal_invert_rt_full(                                      \
        rt_out,                                                \
        0,                                                     \
        dtout_drin,                                            \
        0,                                                     \
        0,                                                     \
        dtout_dtin,                                            \
        0,                                                     \
        0,                                                     \
        rt_in,                                                 \
        0                                                      \
    )
void mrcal_invert_rt_full(   // output
    double* rt_out,          // (6,) array
    int rt_out_stride0,      // in bytes. <= 0 means "contiguous"
    double* dtout_drin,      // (3,3) array
    int dtout_drin_stride0,  // in bytes. <= 0 means "contiguous"
    int dtout_drin_stride1,  // in bytes. <= 0 means "contiguous"
    double* dtout_dtin,      // (3,3) array
    int dtout_dtin_stride0,  // in bytes. <= 0 means "contiguous"
    int dtout_dtin_stride1,  // in bytes. <= 0 means "contiguous"

    // input
    const double* rt_in,  // (6,) array
    int rt_in_stride0     // in bytes. <= 0 means "contiguous"
);

// Compose two Rt transformations
//
// Rt = Rt0 * Rt1  --->  Rt(x) = Rt0( Rt1(x) )
//
// The input transformations are given in (4,3) arrays Rt_0 and Rt_1
//
// The result is returned in a (4,3) array Rt_out
//
// In-place operation is supported; the output array may be the same as either
// of the input arrays to overwrite the input.
#define mrcal_compose_Rt(Rt_out, Rt_0, Rt_1) \
    mrcal_compose_Rt_full(Rt_out, 0, 0, Rt_0, 0, 0, Rt_1, 0, 0, false, false)
#define mrcal_compose_Rt_inverted0(Rt_out, Rt_0, Rt_1) \
    mrcal_compose_Rt_full(Rt_out, 0, 0, Rt_0, 0, 0, Rt_1, 0, 0, true, false)
#define mrcal_compose_Rt_inverted1(Rt_out, Rt_0, Rt_1) \
    mrcal_compose_Rt_full(Rt_out, 0, 0, Rt_0, 0, 0, Rt_1, 0, 0, false, true)
#define mrcal_compose_Rt_inverted01(Rt_out, Rt_0, Rt_1) \
    mrcal_compose_Rt_full(Rt_out, 0, 0, Rt_0, 0, 0, Rt_1, 0, 0, true, true)
void mrcal_compose_Rt_full(  // output
    double* Rt_out,          // (4,3) array
    int Rt_out_stride0,      // in bytes. <= 0 means "contiguous"
    int Rt_out_stride1,      // in bytes. <= 0 means "contiguous"

    // input
    const double* Rt_0,  // (4,3) array
    int Rt_0_stride0,    // in bytes. <= 0 means "contiguous"
    int Rt_0_stride1,    // in bytes. <= 0 means "contiguous"
    const double* Rt_1,  // (4,3) array
    int Rt_1_stride0,    // in bytes. <= 0 means "contiguous"
    int Rt_1_stride1,    // in bytes. <= 0 means "contiguous"
    bool inverted0,
    bool inverted1
);

// Compose two rt transformations
//
// rt = rt0 * rt1  --->  rt(x) = rt0( rt1(x) )
//
// The input transformations are given in (6,) arrays rt_0 and rt_1
//
// The result is returned in a (6,) array rt_out
//
// The gradient dr/dr0 is returned in a (3,3) array dr_dr0. Set to NULL if this
// is not wanted
//
// The gradient dr/dr1 is returned in a (3,3) array dr_dr1. Set to NULL if this
// is not wanted
//
// The gradient dt/dr0 is returned in a (3,3) array dt_dr0. Set to NULL if this
// is not wanted
//
// The gradient dt/dt1 is returned in a (3,3) array dt_dt1. Set to NULL if this
// is not wanted
//
// The gradients dr/dt0, dr/dt1 are always 0, so they are never returned
//
// If neither of the inputs is inverted, the dt/dr1 = 0 always and dt/dt0 = I
// always, so the convenience macro mrcal_compose_rt() doesn't return those. The
// other macros and mrcal_compose_rt_full() do report those as well
//
// In-place operation is supported; the output array may be the same as either
// of the input arrays to overwrite the input.
#define mrcal_compose_rt(rt_out, dr_dr0, dr_dr1, dt_dr0, dt_dt1, rt_0, rt_1) \
    mrcal_compose_rt_full(                                                   \
        rt_out,                                                              \
        0,                                                                   \
        dr_dr0,                                                              \
        0,                                                                   \
        0,                                                                   \
        dr_dr1,                                                              \
        0,                                                                   \
        0,                                                                   \
        dt_dr0,                                                              \
        0,                                                                   \
        0,                                                                   \
        NULL,                                                                \
        0,                                                                   \
        0,                                                                   \
        NULL,                                                                \
        0,                                                                   \
        0,                                                                   \
        dt_dt1,                                                              \
        0,                                                                   \
        0,                                                                   \
        rt_0,                                                                \
        0,                                                                   \
        rt_1,                                                                \
        0,                                                                   \
        false,                                                               \
        false                                                                \
    )
#define mrcal_compose_rt_inverted0( \
    rt_out,                         \
    dr_dr0,                         \
    dr_dr1,                         \
    dt_dr0,                         \
    dt_dr1,                         \
    dt_dt0,                         \
    dt_dt1,                         \
    rt_0,                           \
    rt_1                            \
)                                   \
    mrcal_compose_rt_full(          \
        rt_out,                     \
        0,                          \
        dr_dr0,                     \
        0,                          \
        0,                          \
        dr_dr1,                     \
        0,                          \
        0,                          \
        dt_dr0,                     \
        0,                          \
        0,                          \
        dt_dr1,                     \
        0,                          \
        0,                          \
        dt_dt0,                     \
        0,                          \
        0,                          \
        dt_dt1,                     \
        0,                          \
        0,                          \
        rt_0,                       \
        0,                          \
        rt_1,                       \
        0,                          \
        true,                       \
        false                       \
    )
#define mrcal_compose_rt_inverted1( \
    rt_out,                         \
    dr_dr0,                         \
    dr_dr1,                         \
    dt_dr0,                         \
    dt_dr1,                         \
    dt_dt0,                         \
    dt_dt1,                         \
    rt_0,                           \
    rt_1                            \
)                                   \
    mrcal_compose_rt_full(          \
        rt_out,                     \
        0,                          \
        dr_dr0,                     \
        0,                          \
        0,                          \
        dr_dr1,                     \
        0,                          \
        0,                          \
        dt_dr0,                     \
        0,                          \
        0,                          \
        dt_dr1,                     \
        0,                          \
        0,                          \
        dt_dt0,                     \
        0,                          \
        0,                          \
        dt_dt1,                     \
        0,                          \
        0,                          \
        rt_0,                       \
        0,                          \
        rt_1,                       \
        0,                          \
        false,                      \
        true                        \
    )
#define mrcal_compose_rt_inverted01( \
    rt_out,                          \
    dr_dr0,                          \
    dr_dr1,                          \
    dt_dr0,                          \
    dt_dr1,                          \
    dt_dt0,                          \
    dt_dt1,                          \
    rt_0,                            \
    rt_1                             \
)                                    \
    mrcal_compose_rt_full(           \
        rt_out,                      \
        0,                           \
        dr_dr0,                      \
        0,                           \
        0,                           \
        dr_dr1,                      \
        0,                           \
        0,                           \
        dt_dr0,                      \
        0,                           \
        0,                           \
        dt_dr1,                      \
        0,                           \
        0,                           \
        dt_dt0,                      \
        0,                           \
        0,                           \
        dt_dt1,                      \
        0,                           \
        0,                           \
        rt_0,                        \
        0,                           \
        rt_1,                        \
        0,                           \
        true,                        \
        true                         \
    )
void mrcal_compose_rt_full(  // output
    double* rt_out,          // (6,) array
    int rt_out_stride0,      // in bytes. <= 0 means "contiguous"
    double* dr_dr0,          // (3,3) array; may be NULL
    int dr_dr0_stride0,      // in bytes. <= 0 means "contiguous"
    int dr_dr0_stride1,      // in bytes. <= 0 means "contiguous"
    double* dr_dr1,          // (3,3) array; may be NULL
    int dr_dr1_stride0,      // in bytes. <= 0 means "contiguous"
    int dr_dr1_stride1,      // in bytes. <= 0 means "contiguous"
    double* dt_dr0,          // (3,3) array; may be NULL
    int dt_dr0_stride0,      // in bytes. <= 0 means "contiguous"
    int dt_dr0_stride1,      // in bytes. <= 0 means "contiguous"
    double* dt_dr1,          // (3,3) array; may be NULL
    int dt_dr1_stride0,      // in bytes. <= 0 means "contiguous"
    int dt_dr1_stride1,      // in bytes. <= 0 means "contiguous"
    double* dt_dt0,          // (3,3) array; may be NULL
    int dt_dt0_stride0,      // in bytes. <= 0 means "contiguous"
    int dt_dt0_stride1,      // in bytes. <= 0 means "contiguous"
    double* dt_dt1,          // (3,3) array; may be NULL
    int dt_dt1_stride0,      // in bytes. <= 0 means "contiguous"
    int dt_dt1_stride1,      // in bytes. <= 0 means "contiguous"

    // input
    const double* rt_0,  // (6,) array
    int rt_0_stride0,    // in bytes. <= 0 means "contiguous"
    const double* rt_1,  // (6,) array
    int rt_1_stride0,    // in bytes. <= 0 means "contiguous"
    bool inverted0,
    bool inverted1
);

// Compose two angle-axis rotations
//
// r = r0 * r1  --->  r(x) = r0( r1(x) )
//
// The input rotations are given in (3,) arrays r_0 and r_1
//
// The result is returned in a (3,) array r_out
//
// The gradient dr/dr0 is returned in a (3,3) array dr_dr0. Set to NULL if this
// is not wanted
//
// The gradient dr/dr1 is returned in a (3,3) array dr_dr1. Set to NULL if this
// is not wanted
//
// In-place operation is supported; the output array may be the same as either
// of the input arrays to overwrite the input.
#define mrcal_compose_r(r_out, dr_dr0, dr_dr1, r_0, r_1) \
    mrcal_compose_r_full(                                \
        r_out,                                           \
        0,                                               \
        dr_dr0,                                          \
        0,                                               \
        0,                                               \
        dr_dr1,                                          \
        0,                                               \
        0,                                               \
        r_0,                                             \
        0,                                               \
        r_1,                                             \
        0,                                               \
        false,                                           \
        false                                            \
    )
#define mrcal_compose_r_inverted0(r_out, dr_dr0, dr_dr1, r_0, r_1) \
    mrcal_compose_r_full(                                          \
        r_out,                                                     \
        0,                                                         \
        dr_dr0,                                                    \
        0,                                                         \
        0,                                                         \
        dr_dr1,                                                    \
        0,                                                         \
        0,                                                         \
        r_0,                                                       \
        0,                                                         \
        r_1,                                                       \
        0,                                                         \
        true,                                                      \
        false                                                      \
    )
#define mrcal_compose_r_inverted1(r_out, dr_dr0, dr_dr1, r_0, r_1) \
    mrcal_compose_r_full(                                          \
        r_out,                                                     \
        0,                                                         \
        dr_dr0,                                                    \
        0,                                                         \
        0,                                                         \
        dr_dr1,                                                    \
        0,                                                         \
        0,                                                         \
        r_0,                                                       \
        0,                                                         \
        r_1,                                                       \
        0,                                                         \
        false,                                                     \
        true                                                       \
    )
#define mrcal_compose_r_inverted01(r_out, dr_dr0, dr_dr1, r_0, r_1) \
    mrcal_compose_r_full(                                           \
        r_out,                                                      \
        0,                                                          \
        dr_dr0,                                                     \
        0,                                                          \
        0,                                                          \
        dr_dr1,                                                     \
        0,                                                          \
        0,                                                          \
        r_0,                                                        \
        0,                                                          \
        r_1,                                                        \
        0,                                                          \
        true,                                                       \
        true                                                        \
    )
void mrcal_compose_r_full(  // output
    double* r_out,          // (3,) array
    int r_out_stride0,      // in bytes. <= 0 means "contiguous"
    double* dr_dr0,         // (3,3) array; may be NULL
    int dr_dr0_stride0,     // in bytes. <= 0 means "contiguous"
    int dr_dr0_stride1,     // in bytes. <= 0 means "contiguous"
    double* dr_dr1,         // (3,3) array; may be NULL
    int dr_dr1_stride0,     // in bytes. <= 0 means "contiguous"
    int dr_dr1_stride1,     // in bytes. <= 0 means "contiguous"

    // input
    const double* r_0,  // (3,) array
    int r_0_stride0,    // in bytes. <= 0 means "contiguous"
    const double* r_1,  // (3,) array
    int r_1_stride0,    // in bytes. <= 0 means "contiguous"
    bool inverted0,
    bool inverted1
);

// Special-case rotation compositions for the uncertainty computation
//
// Same as mrcal_compose_r() except
//
// - r0 is assumed to be 0, so we don't ingest it, and we don't report the
//   composition result
// - we ONLY report the dr01/dr0 gradient
//
// In python this function is equivalent to
//
//   _,dr01_dr0,_ = compose_r(np.zeros((3,),),
//                            r1,
//                            get_gradients=True)
#define mrcal_compose_r_tinyr0_gradientr0(dr_dr0, r_1) \
    mrcal_compose_r_tinyr0_gradientr0_full(dr_dr0, 0, 0, r_1, 0)
void mrcal_compose_r_tinyr0_gradientr0_full(  // output
    double* dr_dr0,                           // (3,3) array; may be NULL
    int dr_dr0_stride0,  // in bytes. <= 0 means "contiguous"
    int dr_dr0_stride1,  // in bytes. <= 0 means "contiguous"

    // input
    const double* r_1,  // (3,) array
    int r_1_stride0     // in bytes. <= 0 means "contiguous"
);
// Same as mrcal_compose_r() except
//
// - r1 is assumed to be 0, so we don't ingest it, and we don't report the
//   composition result
// - we ONLY report the dr01/dr1 gradient
//
// In python this function is equivalent to
//
//   _,_,dr01_dr1 = compose_r(r0,
//                            np.zeros((3,),),
//                            get_gradients=True)
#define mrcal_compose_r_tinyr1_gradientr1(dr_dr1, r_0) \
    mrcal_compose_r_tinyr1_gradientr1_full(dr_dr1, 0, 0, r_0, 0)
void mrcal_compose_r_tinyr1_gradientr1_full(  // output
    double* dr_dr1,                           // (3,3) array; may be NULL
    int dr_dr1_stride0,  // in bytes. <= 0 means "contiguous"
    int dr_dr1_stride1,  // in bytes. <= 0 means "contiguous"

    // input
    const double* r_0,  // (3,) array
    int r_0_stride0     // in bytes. <= 0 means "contiguous"
);

// Procrustes fit functions. Align two corresponding sets of normalized
// direction vectors or points. Return true on success
bool mrcal_align_procrustes_vectors_R01(  // out
    double* R01,
    // in
    const int N,
    // (N,3) arrays
    // normalized direction vectors
    const double* v0,
    const double* v1,

    // (N,) array; may be NULL to use an even
    // weighting
    const double* weights
);

bool mrcal_align_procrustes_points_Rt01(  // out
    double* Rt01,
    // in
    const int N,
    // (N,3) arrays
    const double* p0,
    const double* p1,

    // (N,) array; may be NULL to use an even
    // weighting
    const double* weights
);

// Compute a non-unique rotation to map a given vector to [0,0,1]
void mrcal_R_aligned_to_vector(  // out
    double* R,
    // in
    const double* v
);
