
// angle_range.cpp - routines for manipulating angle ranges

#include <assert.h>
#include <stdio.h>
#define _USE_MATH_DEFINES  // so Windows version of <cmath> & <math.h> have M_PI
#include <cmath>

#include <Cg/degrees.hpp>
#include <Cg/radians.hpp>

#include "angle_range.hpp"

using Cg::degrees;
using Cg::radians;

float normalizeAngle(float angle) 
{
    while (angle < -M_PI) {
        angle += float(2*M_PI);
    }
    while (angle >  M_PI) {
        angle -= float(2*M_PI);
    }
    return angle;
}

float absoluteNormalizeAngle(float angle)
{
    return fabs(normalizeAngle(angle));
}

bool isWithinRange(float theta, float a, float b)
{
    // Rotate angles theta, a, and b so theta is at zero.
    const float adjusted_theta = theta - theta;
    const float adjusted_a     = a     - theta;
    const float adjusted_b     = b     - theta;
    // Normalize adjusted a & b to the range (-M_PI,M_PI).
    const float normalized_a = normalizeAngle(adjusted_a);
    const float normalized_b = normalizeAngle(adjusted_b);

    // Are normalized a & b on opposite sides of adjusted theta (i.e. zero)?
    if (normalized_a*normalized_b < 0) {
        // Yes, so compute the angle span between normalized a and b.
        const float angle_span = fabs(normalized_a - normalized_b);
        // Does the arc between a and b containing theta form
        // a reflex angle (i.e. more than M_PI degrees)?
        if (angle_span >= M_PI) {
            // Yes, then theta is NOT within the angular range of a and b.
            return false;
        } else {
            // No, then theta IS within the angular range of a and b.
            return true;
        }
    } else {
        // No, so theta is not between a and b.
        return false;
    }
}

bool isWithinOrientedRange(float theta, float a, float b, float winding)
{
    // Rotate angles theta, a, and b so theta is at zero.
    const float adjusted_theta = theta - theta;  // should be zero!
    const float adjusted_a     = a     - theta;
    const float adjusted_b     = b     - theta;
    // Normalize adjusted a & b to the range (-M_PI,M_PI).
    const float normalized_a = normalizeAngle(adjusted_a);
    const float normalized_b = normalizeAngle(adjusted_b);
    // theta is within the oriented (+1 winding for clockwise;
    // -1 winding for counter-clockwise) range of a & b when
    // an odd number of the following expressions are true.
    const bool e1 = winding > 0;
    const bool e2 = normalized_b > normalized_a;
    const bool e3 = normalized_a > 0;
    const bool e4 = normalized_b > 0;
    const bool is_within_range = e1 ^ e2 ^ e3 ^ e4;
    return is_within_range;
}

float clampToOrientedRange(float theta, float a, float b, float winding)
{
    if (isWithinOrientedRange(theta, a, b, winding)) {
        return theta;
    } else {
        // Rotate angles theta, a, and b so theta is at zero.
        const float adjusted_theta = theta - theta;
        const float adjusted_a     = a     - theta;
        const float adjusted_b     = b     - theta;
        // Normalize adjusted a & b to the range (-M_PI,M_PI).
        const float normalized_a = normalizeAngle(adjusted_a);
        const float normalized_b = normalizeAngle(adjusted_b);
        const float abs_normalized_a = fabs(normalized_a);
        const float abs_normalized_b = fabs(normalized_b);
        if (abs_normalized_a < abs_normalized_b) {
            return a;
        } else {
            return b;
        }
    }
}

// Regression tests...

static bool try_isWithinRange(float testAngle, float a, float b)
{
    printf("  a         = %g\n", a);
    printf("  testAngle = %g\n", testAngle);
    printf("  b         = %g\n", b);
    testAngle = radians(testAngle);
    a = radians(a);
    b = radians(b);
    bool result = isWithinRange(testAngle, a, b);
    printf("isWithinRange = %d, %s\n", result, result ? "INSIDE" : "outside");
    return result;
}

static bool try_isWithinOrientedRange(float testAngle, float a, float b, float winding)
{
    printf("  a         = %+g\n", a);
    printf("  testAngle = %+g\n", testAngle);
    printf("  b         = %+g\n", b);
    printf("  winding   = %+g, %s\n", winding, winding > 0 ? "counterCW" : "CW");
    testAngle = radians(testAngle);
    a = radians(a);
    b = radians(b);
    bool result = isWithinOrientedRange(testAngle, a, b, winding);
    printf("isWithinOrientedRange = %d, %s\n", result, result ? "INSIDE" : "outside");
    return result;
}

static void try_both(float testAngle, float a, float b)
{
    try_isWithinRange(testAngle, a, b);
    try_isWithinOrientedRange(testAngle, a, b, +1);
    try_isWithinOrientedRange(testAngle, a, b, -1);
    printf("\n");
}

static void test_both(float testAngle, float a, float b, bool expected_r1, bool expected_r2, bool expected_r3)
{
    bool r1 = try_isWithinRange(testAngle, a, b);
    bool r2 = try_isWithinOrientedRange(testAngle, a, b, +1);
    bool r3 = try_isWithinOrientedRange(testAngle, a, b, -1);
    int fail_count = 0;
    if (r1 != expected_r1) {
        printf("         r1: isWithinRange FAILED\n");
        fail_count++;
    }
    if (r2 != expected_r2) {
        printf("          r2: isWithinOrientedRange +1 FAILED\n");
        fail_count++;
    }
    if (r3 != expected_r3) {
        printf("          r3: isWithinOrientedRange -1 FAILED\n");
        fail_count++;
    }
    if (fail_count > 0) {
        printf("FAILED %d\n", fail_count);
    }
    printf("\n");
}

static void test_clampToOrientedRange(float testAngle, float a, float b, float winding, float expecting, float expecting2)
{
    int fail_count = 0;
    float testAngle_rad = radians(testAngle);
    float a_rad = radians(a);
    float b_rad = radians(b);
    float got = clampToOrientedRange(testAngle_rad, a_rad, b_rad, winding);
    float got_deg = degrees(got);
    if (got_deg != expecting) {
        printf("clampToOrientedRange(%g,%g,%g,%g), got %g (%g), expected %g\n", 
            testAngle, a, b, winding, got_deg, got, expecting);
        fail_count++;
    }
    float got2 = clampToOrientedRange(testAngle_rad, a_rad, b_rad, -winding);
    float got2_deg = degrees(got2);
    if (got2_deg != expecting2) {
        printf("clampToOrientedRange(%g,%g,%g,%g), got %g (%g), expected %g\n", 
            testAngle, a, b, -winding, got2_deg, got2, expecting2);
        fail_count++;
    }
    if (fail_count > 0) {
        printf("FAILED %d\n", fail_count);
    }
    printf("\n");
}

#define outside false
#define INSIDE true

void test_isWithinRange()
{
    //                               +1 CCW    -1 CW
    test_both(0, 140, -140, outside, outside,  INSIDE);
    test_both(0,  30,  -30, INSIDE,  outside,  INSIDE);
    test_both(0, -30,   30, INSIDE,   INSIDE, outside);

    test_both(0, -120,   30,  INSIDE, INSIDE, outside);
    test_both(0,   30, -120,  INSIDE, outside, INSIDE);

    test_both(150, 140, -140, INSIDE, INSIDE, outside);

    test_both(170, -120, 80, INSIDE, outside, INSIDE);
    test_both(-10, -120, 80, outside, INSIDE, outside);
    test_both(70, -120, 80,  outside, INSIDE, outside);
    test_both(-170, -120, 80, INSIDE, outside, INSIDE);

    test_both(-170, 30, 40, outside, outside, INSIDE);
    test_both(35, 30, 40, INSIDE, INSIDE, outside);
    test_both(360+50, 30, 40, outside, outside, INSIDE);
    test_both(-50, 30, 40, outside, outside, INSIDE);
    test_both(-50-720, 30, 40, outside, outside, INSIDE);

    test_both(90, 140, -140, outside, outside, INSIDE);

    test_both(360, 140, -140, outside, outside, INSIDE);
    test_both(360, -140, 140, outside, INSIDE, outside);

    test_both(179, 178, -178, INSIDE, INSIDE, outside);
    test_both(179, 178, -1, outside, INSIDE, outside);

    test_both(0, -45, 170, outside, INSIDE, outside);
    test_both(171, -45, 170, INSIDE, outside, INSIDE);
    test_both(169, -45, 170, outside, INSIDE, outside);
    test_both(-46, -45, 170, INSIDE, outside, INSIDE);

    test_clampToOrientedRange(46, 45, 30, -1, 45, 46);
    test_clampToOrientedRange(146, 145, 30, +1, 146, 145);

    test_clampToOrientedRange(179, -145, 30, +1, -145, 179);
    test_clampToOrientedRange(-145, -145, 30, +1, -145, -145);

    try_both(-90, 140, -140);
    try_both(270, 140, -140);
    try_both(normalizeAngle(270), 140, -140);
    try_both(-90, 140, -140);
    try_both(-270, 140, -140);

    printf("done\n");
}
