npc_to_in <- function(xy, x = 0.5, y = 0.5, w = 1, h = 1, t = 0) {
	xy$translate(as_coord2d(-0.5, -0.5))$scale(w, h)$rotate(degrees(t))$translate(as_coord2d(x, y))
	invisible(xy)
}


# Name 'nigh' to avoid potential conflict with 'dplyr::near()'
nigh <- function(x, y, tolerance = 1e-6) {
	if (length(y) < length(x)) {
		y <- rep(y, length.out = length(x))
	}
	isTRUE(all.equal(x, y, tolerance = tolerance))
}

# robust version of `acos()` that returns in degrees
arccos <- function(x) {
	affiner::arccosine(x, "degrees")
}

# Axis-angle representation to rotation matrix
# https://en.wikipedia.org/wiki/Axis-angle_representation
# Because we do rotation matrix post-multiplication instead of pre-multiplication we usually need to multiply angles
# in following algorithms by -1

#' @rdname geometry_utils
#' @inheritParams save_piece_obj
#' @param axis_z Third coordinate of the axis unit vector (usually inferred).
#' @param angle Numeric vector in degrees (counter-clockwise) or an [affiner::angle()] vector.
#' @param ... Ignored
#' @export
AA_to_R <- function(angle = 0, axis_x = 0, axis_y = 0, axis_z = NA, ...) {
	if (is.na(axis_z)) {
		inner <- 1 - axis_x^2 - axis_y^2
		if (nigh(inner, 0)) {
			axis_z <- 0
		} else {
			axis_z <- sqrt(inner)
		}
	}
	axis <- as_coord3d(axis_x, axis_y, axis_z)
	affiner::rotate3d(axis, degrees(angle))[1:3, 1:3]
}

# Rotation matrix to Axis-angle representation
# https://en.wikipedia.org/wiki/Axis-angle_representation
#' @param R 3D rotation matrix (post-multiplied)
#' @rdname geometry_utils
#' @export
R_to_AA <- function(R = diag(3)) {
	l <- affiner::rotate3d_to_AA(R, unit = "degrees")
	list(angle = as.numeric(l$theta), axis_x = l$axis$x, axis_y = l$axis$y, axis_z = l$axis$z)
}

# Basic 3D rotation matrices
# https://en.wikipedia.org/wiki/Rotation_matrix#In_three_dimensions

#' Helper functions for making geometric calculations.
#'
#' `to_x()`, `to_y()`, `to_r()`, `to_t()` convert
#' between polar coordinates (in degrees) and Cartesian coordinates.
#' `to_degrees()` and `to_radians()` converts between degrees and radians.
#' `AA_to_R()` and `R_to_AA()` convert back and forth between (post-multiplied) rotation matrix
#' and axis-angle representations of 3D rotations.
#' `R_x()`, `R_y()`, and `R_z()` build (post-multiplied) rotation matrices for simple rotations around
#' the x, y, and z axes.
#'
#' [pp_cfg()] uses polar coordinates to determine where the "primary" and "directional"
#' symbols are located on a game piece.
#' They are also useful for drawing certain shapes and for making game diagrams on hex boards.
#'
#' `piecepackr` and `grid` functions use angles in degrees
#'  but the `base` trigonometry functions usually use radians.
#'
#' `piecepackr`'s 3D graphics functions [save_piece_obj()], [piece()], [piece3d()], and [piece_mesh()]
#' use the axis-angle representation for 3D rotations.
#' The axis-angle representation involves specifying a unit vector
#' indicating the direction of an axis of rotation and an angle describing the (counter-clockwise)
#' rotation around that axis.  Because it is a unit vector one only needs to specify the first two elements,
#' `axis_x` and `axis_y`, and we are able to infer the 3rd element `axis_z`.  The default of
#' `axis = 0`, `axis_y = 0`, and implied `axis_z = 1`
#' corresponds to a rotation around the z-axis which is reverse-compatible
#' with the originally 2D `angle` interpretation in [grid.piece()].  In order to figure out the appropriate
#' axis-angle representation parameters `R_to_AA()`, `R_x()`, `R_y()`, and `R_z()` allow one
#' to first come up with an appropriate (post-multiplied) 3D rotation matrix by chaining simple rotations
#' and then convert them to the corresponding axis-angle representation.
#' Pieces are rotated as if their center was at the origin.
#'
#' @examples
#'  to_x(90, 1)
#'  to_y(180, 0.5)
#'  to_t(0, -1)
#'  to_r(0.5, 0)
#'  all.equal(pi, to_radians(to_degrees(pi)))
#'  # default axis-angle axis is equivalent to a rotation about the z-axis
#'  all.equal(AA_to_R(angle=60), R_z(angle=60))
#'  # axis-angle representation of 90 rotation about the x-axis
#'  R_to_AA(R_x(90))
#'  # find Axis-Angle representation of first rotating about x-axis 180 degrees
#'  # and then rotating about z-axis 45 degrees
#'  R_to_AA(R_x(180) %*% R_z(45))
#'
#' @seealso \url{https://en.wikipedia.org/wiki/Axis-angle_representation} for more details
#'   about the Axis-angle representation of 3D rotations.
#'   See \code{\link[base]{Trig}} for R's built-in trigonometric functions.
#' @name geometry_utils
NULL

#' @rdname geometry_utils
#' @export
R_x <- function(angle = 0) {
	c <- affiner::cosine(degrees(-angle))
	s <- affiner::sine(degrees(-angle))
	matrix(c(1, 0, 0, 0, c, -s, 0, s, c), ncol = 3, byrow = TRUE)
}

#' @rdname geometry_utils
#' @export
R_y <- function(angle = 0) {
	c <- affiner::cosine(degrees(-angle))
	s <- affiner::sine(degrees(-angle))
	matrix(c(c, 0, s, 0, 1, 0, -s, 0, c), ncol = 3, byrow = TRUE)
}

#' @rdname geometry_utils
#' @export
R_z <- function(angle = 0) {
	c <- affiner::cosine(degrees(-angle))
	s <- affiner::sine(degrees(-angle))
	matrix(c(c, -s, 0, s, c, 0, 0, 0, 1), ncol = 3, byrow = TRUE)
}

#' @rdname geometry_utils
#' @export
to_radians <- function(t) as.numeric(affiner::radians(degrees(t)))

#' @rdname geometry_utils
#' @export
to_degrees <- function(t) as.numeric(degrees(affiner::radians(t)))

#' @rdname geometry_utils
#' @param t Numeric vector in degrees (or radians for `to_degrees()`) or an [affiner::angle()] vector.
#' @param r Radial distance
#' @export
to_x <- function(t, r) {
	r * affiner::cosine(degrees(t))
}

#' @rdname geometry_utils
#' @export
to_y <- function(t, r) {
	r * affiner::sine(degrees(t))
}

#' @rdname geometry_utils
#' @param x Cartesian x coordinate
#' @param y Cartesian y coordinate
#' @export
to_r <- function(x, y) {
	sqrt(x^2 + y^2)
}

#' @rdname geometry_utils
#' @export
to_t <- function(x, y) {
	180 * atan2(y, x) / pi
}
