// import a hash map implementation
use fnv::FnvHashMap;
use std::f64;

// these are multiplied by epsilon to obtain
// smaller values of espilon. Both should be
// between 0 and 1, and CHECK_FACTOR should be greater
// than EPS_FACTOR. There is an optimal value of EPS_FACTOR
// for any function, and increasing CHECK_FACTOR produces slightly
// better results but causes the algorithm to take longer.
const EPS_FACTOR: f64 = 1.0 - 1.0 / 4.0;
const CHECK_FACTOR: f64 = 1.0 - 1.0 / 8.0;

// whether to display the entire output or just the number
// of triangles
const SHOW_OUTPUT: bool = true;

// tolerance used for numerical methods
const TOL: f64 = 1.0e-15;

fn main() {
    // the function and its first two derivatives
    let f = |x: f64, y: f64| x.sin() + y.sin();
    let d = |x: f64, y: f64| [x.cos(), y.cos()];
    // [df/dxdx, df/dydy, df/dxdy]
    let dd = |x: f64, y: f64| [-x.sin(), -y.sin(), 0.0];
    let mut v = Vec::new();
    // the points that form the initial two triangles
    let mut points = vec![[-8.0, -8.0], [8.0, -8.0], [8.0, 8.0], [-8.0, 8.0]];
    // generate the approximation
    generate_approx_2d(
        // the function will add more values to points
        &mut points,
        // the indices of the triangles to generate
        // we can't generate the two triangles seperately
        // because the seam between them wouldn't line up
        &[[0, 1, 2], [2, 3, 0]],
        // epsilon
        1.0 / 8.0,
        // the step size. For any function g(x)=f(x0+ax, y0+bx), where
        // a^2+b^2=1, g can be decomposed into [p_i, q_i] intervals where
        // either g or -g is convex on [p_i, q_i] with q_i - p_i > step.
        // I am not sure if there is an easy way to find this constant
        1.0,
        // a value that should be larger than the lipschitz constant
        // for the function (used to create an oracle)
        1.1,
        // the function will add sets of indices to this vector
        &mut v,
        // pass the lambdas for the function
        f,
        d,
        dd,
    );
    // display the number of triangles generated
    println!("{}", v.len());
    if SHOW_OUTPUT {
        print!("[");
        for p in points {
            for val in &p {
                print!("{:.12}, ", val);
            }
        }
        println!("]");
        print!("[");
        for tri in v {
            for index in &tri {
                print!("{}, ", index);
            }
        }
        println!("]");
    }
}

// generate a piecewise linear approximation of
// a function of two variables. Note that the
// vertices of the simplices generated lie on the function
fn generate_approx_2d<
    F: Fn(f64, f64) -> f64 + Copy,
    D: Fn(f64, f64) -> [f64; 2] + Copy,
    // [df/dxdx, df/dydy, df/dxdy]
    D2: Fn(f64, f64) -> [f64; 3] + Copy,
>(
    points: &mut Vec<[f64; 2]>,
    tris: &[[usize; 3]],
    epsilon: f64,
    step: f64,
    k: f64,
    v: &mut Vec<[usize; 3]>,
    f: F,
    d: D,
    dd: D2,
) {
    let small_epsilon = epsilon * EPS_FACTOR;
    let len = tris.len();
    // initialize a hash map
    let mut edges_map = FnvHashMap::with_capacity_and_hasher(len * 2, Default::default());

    // a lambda for processing the edges. This makes sure that edges between
    // triangles line up
    let mut get_edge = |n1: usize, n2: usize| {
        let (p1, p2, ccw) = if n1 > n2 {
            (n2, n1, false)
        } else {
            (n1, n2, true)
        };
        let (line, dist) = norm_line(points[p1][0], points[p1][1], points[p2][0], points[p2][1]);
        if let Some((start, end)) = edges_map.get(&(p1, p2)) {
            ((*start, *end), ccw, line)
        } else {
            let start = points.len();

            let vec = generate_approx_1d(
                0.0,
                dist,
                small_epsilon,
                step,
                |x| line.val(x, f),
                |x| line.der(x, d),
                |x| line.der_2(x, dd),
            );

            for t in &vec {
                let point = line.pos(*t);
                points.push([point.0, point.1]);
            }

            let end = start + vec.len();
            edges_map.insert((p1, p2), (start, end));
            ((start, end), ccw, line)
        }
    };

    let mut calls = Vec::with_capacity(len);

    for i in 0..len {
        let [n1, n2, n3] = tris[i];
        let (i1, c1, _) = get_edge(n1, n2);
        let (i2, c2, _) = get_edge(n2, n3);
        let (i3, c3, _) = get_edge(n3, n1);
        calls.push(([n1, n2, n3], [i1, i2, i3], [c1, c2, c3]));
    }
    generate_2d_approx_loop(epsilon, step, k, points, v, &mut calls, f, d, dd);
}

// originally I implemented a recursive algorithm, but I was
// getting stack overflows so I changed it to use a heap-allocated
// list of calls
fn generate_2d_approx_loop<
    F: Fn(f64, f64) -> f64 + Copy,
    D: Fn(f64, f64) -> [f64; 2] + Copy,
    // [df/dxdx, df/dydy, df/dxdy]
    D2: Fn(f64, f64) -> [f64; 3] + Copy,
>(
    epsilon: f64,
    step: f64,
    k: f64,
    points: &mut Vec<[f64; 2]>,
    // the working vector of triangles
    v: &mut Vec<[usize; 3]>,
    // this will contain the initial calls needed
    stack: &mut Vec<([usize; 3], [(usize, usize); 3], [bool; 3])>,
    f: F,
    d: D,
    dd: D2,
) {
    while let Some((bound, edges, ccw)) = stack.pop() {
        generate_2d_approx_division(
            bound,
            edges,
            ccw,
            epsilon,
            step,
            k,
            v,
            points,
            stack,
            EPS_FACTOR,
            CHECK_FACTOR,
            f,
            d,
            dd,
        );
    }
}

fn generate_2d_approx_division<
    F: Fn(f64, f64) -> f64 + Copy,
    D: Fn(f64, f64) -> [f64; 2] + Copy,
    // [df/dxdx, df/dydy, df/dxdy]
    D2: Fn(f64, f64) -> [f64; 3] + Copy,
>(
    // bound will always be in CCW order
    bound: [usize; 3],
    // the positions of points along each edge. This stores
    // a start and end index into points.
    edges: [(usize, usize); 3],
    // the order of points on an edge might be backward since
    // neighboring triangles share edges
    edge_ccw: [bool; 3],
    epsilon: f64,
    step: f64,
    k: f64,
    v: &mut Vec<[usize; 3]>,
    points: &mut Vec<[f64; 2]>,
    stack: &mut Vec<([usize; 3], [(usize, usize); 3], [bool; 3])>,
    factor: f64,
    check_factor: f64,
    f: F,
    d: D,
    dd: D2,
) {
    let small_epsilon = epsilon * factor;

    // get the total length of all the edges
    let edges_len = edges[0].1 - edges[0].0 + edges[1].1 - edges[1].0 + edges[2].1 - edges[2].0;

    if edges_len == 0 {
        // in this case there is no further subdivision neccesary,
        // so we check if the triangle described by bound
        // is entirely within the epsilon bound.
        let check_epsilon = epsilon * check_factor;
        let [x1, y1] = points[bound[0]];
        let [x2, y2] = points[bound[1]];
        let [x3, y3] = points[bound[2]];
        let (l1, d1) = norm_line(x1, y1, x2, y2);
        let (l2, d2) = norm_line(x2, y2, x3, y3);
        let (l3, d3) = norm_line(x3, y3, x1, y1);
        let lines = [l1, l2, l3];
        let dists = [d1, d2, d3];
        let mut high_dist = d1;
        let mut high_pos = 0;
        for i in 1..3 {
            if dists[i] > high_dist {
                high_dist = dists[i];
                high_pos = i;
            }
        }
        let lines = [
            lines[high_pos],
            lines[(high_pos + 1) % 3],
            lines[(high_pos + 2) % 3],
        ];
        let point = lines[2].pos(0.0);

        let z1 = lines[0].val(0.0, f);
        let z2 = lines[1].val(0.0, f);
        let z_slope = (z2 - z1) / high_dist;
        // the lipschitz constant gives us a range for where
        // the triangle is within the bound given that a line
        // from one vertex to a point on the opposite edge
        // is within a smaller bound
        let first_step_len = 2.0 * (1.0 - EPS_FACTOR) * epsilon / (k + z_slope.abs());
        let other_step_len = 2.0 * (1.0 - CHECK_FACTOR) * epsilon / (k + z_slope.abs());
        let mut check_pos = first_step_len;
        let mut last = true;

        while check_pos < high_dist {
            let check_point = lines[0].pos(check_pos);
            let (line, len) = norm_line(point.0, point.1, check_point.0, check_point.1);
            // check if the line is within the smaller bound
            let check = max_from_point(
                (0.0, line.val(0.0, f)),
                0.0,
                check_epsilon,
                step,
                len,
                |x| line.val(x, f),
                |x| line.der(x, d),
                |x| line.der_2(x, dd),
            );

            if let Err((min_slope, max_slope)) = check {
                let end_z = z1 + check_pos * z_slope;
                let end_slope = (end_z - line.val(0.0, f)) / len;
                if min_slope <= end_slope && end_slope <= max_slope {
                    check_pos += other_step_len;
                    continue;
                }
            }
            // if we reach this point then we need to further subdivide
            // the triangle. This is generally inneficient, and the optimal
            // value of check_factor will generally cause this case to
            // only occur a few times
            last = false;
            let factor = 0.5;
            let center_point = line.pos(len * factor);
            let center_pos = points.len();
            points.push([center_point.0, center_point.1]);
            let (line_0, len_0) = norm_line(lines[0].x, lines[0].y, center_point.0, center_point.1);
            let (line_1, len_1) = norm_line(lines[1].x, lines[1].y, center_point.0, center_point.1);
            let div_lens = [len_0, len_1, len * factor];
            let div_lines = [line_0, line_1, line];
            let mut line_points = [(0, 0); 3];
            for i in 0..3 {
                // it is possible that the lines to the new point
                // aren't within the small_epsilon bound
                let approx = generate_approx_1d(
                    0.0,
                    div_lens[i],
                    small_epsilon,
                    step,
                    |x| div_lines[i].val(x, f),
                    |x| div_lines[i].der(x, d),
                    |x| div_lines[i].der_2(x, dd),
                );
                let index = points.len();
                line_points[i] = (index, index + approx.len());
                for t in &approx {
                    let point = div_lines[i].pos(*t);
                    points.push([point.0, point.1]);
                }
            }
            for i in 0..3 {
                let next = (i + 1) % 3;
                stack.push((
                    [center_pos, bound[i], bound[next]],
                    // the index 3 is an empty vec
                    [line_points[i], (0, 0), line_points[next]],
                    [false, true, true],
                ));
            }
            break;
        }

        if last {
            v.push(bound);
        }
        return;
    }
    // generate a list to help process triangles
    let mut index_vec = Vec::with_capacity(3 + edges_len);
    let mut edge_boundaries = [0; 4];
    for i in 0..3 {
        index_vec.push(bound[i]);
        let edge_len = edges[i].1 - edges[i].0;
        for k in 0..edge_len {
            let n = if edge_ccw[i] {
                edges[i].0 + k
            } else {
                edges[i].1 - 1 - k
            };
            index_vec.push(n);
        }
        edge_boundaries[i + 1] = index_vec.len();
    }
    let triangulation = get_triangulation(edge_boundaries);
    let len = triangulation.len() / 3;
    let mut edges_map = FnvHashMap::with_capacity_and_hasher((3 * len) / 2, Default::default());

    let get_edge =
        |edges_map: &mut FnvHashMap<_, _>, points: &mut Vec<[f64; 2]>, n1: usize, n2: usize| -> ((usize, usize), [f64; 2], f64, bool) {
            let (p1, p2, ccw) = if n1 > n2 {
                (n2, n1, false)
            } else {
                (n1, n2, true)
            };

            // check if the points lie on the same edge
            let mut p1_edge = 0;
            let mut p2_edge = 0;

            for i in 0..3 {
                if p1 < edge_boundaries[3 - i] {
                    p1_edge = 2 - i;
                }
                if p2 > edge_boundaries[i] {
                    p2_edge = i;
                }
            }

            let point1 = points[index_vec[p1]];
            let point2 = points[index_vec[p2]];

            let (edge_line, dist) = norm_line(point1[0], point1[1], point2[0], point2[1]);
            let mid = [(point1[0] + point2[0]) / 2.0, (point1[1] + point2[1]) / 2.0];

            // if both points lie on the same edge, then we don't want to
            // generate a new approx, as that would cause point aliasing
            if (p1_edge == p2_edge) || (p1 == 0 && p2 >= edge_boundaries[2]) {
                if p2 - p1 == 1 || (p1 == 0 && edge_boundaries[3] - p2 == 1) {
                    if let Some((start, end)) = edges_map.get(&(p1, p2)) {
                        // this case occurs when an edge of an obtuse triangle
                        // is subdivided
                        return ((*start, *end), mid, dist, ccw);
                    }
                    return ((0, 0), mid, dist, ccw);
                } else {
                    let (edge, start, end) = if p1 == 0 && p2 >= edge_boundaries[2] {
                        (
                            2,
                            p2 - edge_boundaries[2],
                            edge_boundaries[3] - edge_boundaries[2],
                        )
                    } else {
                        (
                            p1_edge,
                            p1 - edge_boundaries[p1_edge],
                            p2 - edge_boundaries[p1_edge],
                        )
                    };
                    let ccw = edge_ccw[edge];
                    let (index_start, index_end) = if ccw {
                        (edges[edge].0 + start, edges[edge].0 + end - 1)
                    } else {
                        (edges[edge].1 - end + 1, edges[edge].1 - start)
                    };
                    return ((index_start, index_end), mid, dist, ccw);
                }
            }

            // check if the edge has already been processed
            if let Some((start, end)) = edges_map.get(&(p1, p2)) {
                return ((*start, *end), mid, dist, ccw);
            } else {
                let approx = generate_approx_1d(
                    0.0,
                    dist,
                    small_epsilon,
                    step,
                    |x| edge_line.val(x, f),
                    |x| edge_line.der(x, d),
                    |x| edge_line.der_2(x, dd),
                );
                let index = points.len();
                let end = index + approx.len();
                edges_map.insert((p1, p2), (index, end));

                for t in &approx {
                    let point = edge_line.pos(*t);
                    points.push([point.0, point.1]);
                }

                ((index, end), mid, dist, ccw)
            }
        };

    let insert_mid = |edges_map: &mut FnvHashMap<_, _>, n1, n2, index| {
        let (p1, p2) = if n1 > n2 { (n2, n1) } else { (n1, n2) };
        edges_map.insert((p1, p2), (index, index + 1));
    };

    // the first time through is used to check for obtuse triangles
    // which can cause an infinite loop. The adjustments made to
    // fix this need to also change the edges on neighboring triangles
    for i in 0..len {
        let tri = &triangulation[i * 3..(i * 3) + 3];
        let edges = [
            get_edge(&mut edges_map, points, tri[0], tri[1]),
            get_edge(&mut edges_map, points, tri[1], tri[2]),
            get_edge(&mut edges_map, points, tri[2], tri[0]),
        ];
        for k in 0..3 {
            let edgek = edges[k].0;
            if edgek.1 - edgek.0 == 0 {
                let next1 = (k + 1) % 3;
                let next2 = (k + 2) % 3;
                let (i1, mid1, d1, _) = edges[next1];
                let (i2, mid2, d2, _) = edges[next2];
                let mid0 = edges[k].1;
                let d0 = edges[k].2;
                let len1 = i1.1 - i1.0;
                let len2 = i2.1 - i2.0;
                if d1 * d1 + d2 * d2 <= d0 * d0 {
                    if len1 > 0 || len2 > 0 {
                        let index = points.len();
                        points.push(mid0);
                        insert_mid(&mut edges_map, tri[k], tri[next1], index);
                        if len1 == 0 {
                            points.push(mid1);
                            insert_mid(&mut edges_map, tri[next1], tri[next2], index + 1);
                        }
                        if len2 == 0 {
                            points.push(mid2);
                            insert_mid(&mut edges_map, tri[next2], tri[k], index + 1);
                        }
                    }
                    break;
                }
            }
        }
    }

    // now we run through to actually add the calls to the stack,
    // with the adjustments made to the neccesary edges
    for i in 0..len {
        let tri = &triangulation[i * 3..(i * 3) + 3];
        let edges = [
            get_edge(&mut edges_map, points, tri[0], tri[1]),
            get_edge(&mut edges_map, points, tri[1], tri[2]),
            get_edge(&mut edges_map, points, tri[2], tri[0]),
        ];

        let bound = [index_vec[tri[0]], index_vec[tri[1]], index_vec[tri[2]]];
        let (edge1, _, _, ccw1) = edges[0];
        let (edge2, _, _, ccw2) = edges[1];
        let (edge3, _, _, ccw3) = edges[2];
        stack.push((bound, [edge1, edge2, edge3], [ccw1, ccw2, ccw3]));
    }
}

// generates the indices for a triangulation given the number of
// points on each edge
fn get_triangulation(edges: [usize; 4]) -> Vec<usize> {
    let mut v = Vec::with_capacity(3);
    for i in 0..3 {
        v.push((edges[i] + edges[i + 1]) / 2);
    }
    for i in 0..3 {
        let prev = (i + 2) % 3;
        if edges[i + 1] - edges[i] > 1 {
            v.extend_from_slice(&[edges[i], v[i], v[prev]]);
        }
    }
    v
}

// generate a 1 dimensional approximation where the endpoints
// lie on the function. Note that this is not an optimal approximation
fn generate_approx_1d<
    F: Fn(f64) -> f64 + Copy,
    D: Fn(f64) -> f64 + Copy,
    D2: Fn(f64) -> f64 + Copy,
>(
    start: f64,
    end: f64,
    epsilon: f64,
    step: f64,
    f: F,
    d: D,
    dd: D2,
    // the output does not include the first and last points
) -> Vec<f64> {
    // create the list to hold the results
    let mut v = Vec::new();

    let mut prev = max_from_pos(start, start, epsilon, step, end, f, d, dd);

    // loop until we reach the max
    while let Some((_, x, _, _)) = prev {
        if x == end {
            break;
        }
        v.push(x);
        // the end of the previous line tells us the value of s_epsilon to use here
        prev = max_from_pos(x, x, epsilon, step, end, f, d, dd);
    }
    // we go back through the approximation to cause it to be
    // spaces more evenly if possible
    let n = v.len();
    let equal_len = (end - start) / (n + 1) as f64;
    let mut back = max_from_pos(
        -end,
        -end,
        epsilon,
        step,
        -start,
        |x| f(-x),
        |x| -d(-x),
        |x| dd(-x),
    );
    for i in 0..n {
        let min = if let Some((_, neg_x, _, _)) = back {
            -neg_x
        } else {
            start
        };
        let dist = start + (n - i) as f64 * equal_len;
        let mut new_point = v[n - i - 1];
        if dist < new_point {
            new_point = if min < dist { dist } else { min };
        } else {
            break;
        }
        v[n - i - 1] = new_point;
        back = max_from_pos(
            -new_point,
            -new_point,
            epsilon,
            step,
            -start,
            |x| f(-x),
            |x| -d(-x),
            |x| dd(-x),
        );
    }
    return v;
}

// find a line segment within the epsilon bound with endpoints
// on the function
fn max_from_pos<F: Fn(f64) -> f64, D: Fn(f64) -> f64, D2: Fn(f64) -> f64>(
    point: f64,
    start: f64,
    epsilon: f64,
    step: f64,
    max_x: f64,
    f: F,
    d: D,
    dd: D2,
) -> Option<(f64, f64, f64, bool)> {
    let mut test_x = start;
    let mut prev_x;
    let mut steps = 1.0;
    let point_y = f(point);
    let get_slope = |x, y| (y - point_y) / (x - point);
    let mut low_upper = f64::INFINITY;
    let mut upper_point = start;
    let mut upper_rdir = low_upper - d(start);
    let mut high_lower = -f64::INFINITY;
    let upper_start = low_upper;
    let lower_start = high_lower;
    let mut lower_point = start;
    let mut lower_rdir = high_lower - d(start);
    let mut prev_dd = dd(start);
    let mut rdir = 0.0;

    loop {
        prev_x = test_x;
        test_x = start + steps * step;

        if test_x > max_x {
            test_x = max_x;
        }

        let mut new_dd = dd(test_x);

        let mut should_step = true;

        if prev_dd * new_dd < 0.0 {
            test_x = bisect(prev_x, test_x, |x| dd(x), TOL);
            new_dd = 0.0;
            should_step = false;
        }

        let mut test_y = f(test_x);
        let mut slope = get_slope(test_x, test_y);
        let mut new_rdir = slope - d(test_x);
        if new_rdir * rdir < 0.0 {
            test_x = find_root(
                prev_x,
                test_x,
                None,
                |x| get_slope(x, f(x)) - d(x),
                |x| {
                    let y = f(x);
                    let bottom = x - point;
                    (bottom * d(x) - (y - point_y)) / (bottom * bottom) - dd(x)
                },
                TOL,
            );
            test_y = f(test_x);
            new_rdir = 0.0;
            should_step = false;
            slope = get_slope(test_x, test_y);
        }

        if should_step {
            steps += 1.0;
        }
        rdir = new_rdir;

        let upper_slope = get_slope(test_x, test_y + epsilon);
        let lower_slope = get_slope(test_x, test_y - epsilon);

        let dir = d(test_x);
        let new_upper_rdir = upper_slope - dir;
        let new_lower_rdir = lower_slope - dir;

        let mut new_low_upper = low_upper;
        let mut new_high_lower = high_lower;
        let mut new_lower_point = lower_point;
        let mut new_upper_point = upper_point;

        if upper_rdir > 0.0 && new_upper_rdir < 0.0 {
            let find = bisect(
                prev_x,
                test_x,
                |x| {
                    if x == point {
                        upper_start - d(x)
                    } else {
                        get_slope(x, f(x) + epsilon) - d(x)
                    }
                },
                TOL,
            );
            let find_dir = get_slope(find, f(find) + epsilon);
            if find_dir <= low_upper {
                new_low_upper = find_dir;
                new_upper_point = find;
            }
        }

        if lower_rdir < 0.0 && new_lower_rdir > 0.0 {
            let find = bisect(
                prev_x,
                test_x,
                |x| {
                    if x == point {
                        lower_start - d(x)
                    } else {
                        get_slope(x, f(x) - epsilon) - d(x)
                    }
                },
                TOL,
            );
            let find_dir = get_slope(find, f(find) - epsilon);
            if find_dir >= high_lower {
                new_high_lower = find_dir;
                new_lower_point = find;
            }
        }

        low_upper = new_low_upper;
        high_lower = new_high_lower;
        lower_point = new_lower_point;
        upper_point = new_upper_point;

        if slope == low_upper {
            return Some((low_upper, test_x, upper_point, true));
        }

        if slope == high_lower {
            return Some((high_lower, test_x, lower_point, false));
        }

        if slope > low_upper {
            let find_x = find_root(
                prev_x,
                test_x,
                None,
                |x| {
                    if x == start {
                        d(x) - low_upper
                    } else {
                        get_slope(x, f(x)) - low_upper
                    }
                },
                |x| {
                    let y = f(x);
                    let bottom = x - point;
                    (bottom * d(x) - (y - point_y)) / (bottom * bottom)
                },
                TOL,
            );
            return Some((low_upper, find_x, upper_point, true));
        }

        if slope < high_lower {
            let find_x = find_root(
                prev_x,
                test_x,
                None,
                |x| {
                    if x == start {
                        d(x) - high_lower
                    } else {
                        get_slope(x, f(x)) - high_lower
                    }
                },
                |x| {
                    let y = f(x);
                    let bottom = x - point;
                    (bottom * d(x) - (y - point_y)) / (bottom * bottom)
                },
                TOL,
            );
            return Some((high_lower, find_x, lower_point, false));
        }

        if test_x == max_x {
            return None;
        }

        upper_rdir = new_upper_rdir;
        lower_rdir = new_lower_rdir;
        prev_dd = new_dd;
    }
}

// generate a line and normalize the vector
fn norm_line(x1: f64, y1: f64, x2: f64, y2: f64) -> (Line, f64) {
    let dx = x2 - x1;
    let dy = y2 - y1;
    let dist = (dx * dx + dy * dy).sqrt();
    let l = Line {
        x: x1,
        y: y1,
        dir_x: dx / dist,
        dir_y: dy / dist,
    };
    (l, dist)
}

// used in the oracle
fn max_from_point<F: Fn(f64) -> f64, D: Fn(f64) -> f64, D2: Fn(f64) -> f64>(
    point: (f64, f64),
    start: f64,
    epsilon: f64,
    step: f64,
    max_x: f64,
    f: F,
    d: D,
    dd: D2,
) -> Result<(f64, f64, f64, bool), (f64, f64)> {
    let mut test_x = start;
    let mut prev_x;
    let mut steps = 1.0;
    let get_slope = |x, y| (y - point.1) / (x - point.0);
    let mut low_upper = if start == point.0 {
        if (f(start) + epsilon) == point.1 {
            d(start)
        } else {
            f64::INFINITY
        }
    } else {
        get_slope(start, f(start) + epsilon)
    };
    let mut upper_point = start;
    let mut upper_rdir = low_upper - d(start);
    let mut high_lower = if start == point.0 {
        if (f(start) - epsilon) == point.1 {
            d(start)
        } else {
            -f64::INFINITY
        }
    } else {
        get_slope(start, f(start) - epsilon)
    };
    let upper_start = low_upper;
    let lower_start = high_lower;
    let mut lower_point = start;
    let mut lower_rdir = high_lower - d(start);
    let mut prev_dd = dd(start);

    let hit_upper;

    loop {
        prev_x = test_x;
        test_x = start + steps * step;

        if test_x > max_x {
            test_x = max_x;
        }

        let mut new_dd = dd(test_x);

        if prev_dd * new_dd < 0.0 {
            test_x = bisect(prev_x, test_x, |x| dd(x), TOL);
            new_dd = 0.0;
        } else {
            steps += 1.0;
        }

        let test_y = f(test_x);
        let upper_slope = get_slope(test_x, test_y + epsilon);
        let lower_slope = get_slope(test_x, test_y - epsilon);
        let dir = d(test_x);
        let new_upper_rdir = upper_slope - dir;
        let new_lower_rdir = lower_slope - dir;

        let mut new_low_upper = low_upper;
        let mut new_high_lower = high_lower;
        let mut new_lower_point = lower_point;
        let mut new_upper_point = upper_point;

        let mut found_upper = false;
        let mut found_lower = false;

        if upper_slope <= low_upper {
            new_low_upper = upper_slope;
            new_upper_point = test_x;
        }
        if upper_rdir > 0.0 && new_upper_rdir < 0.0 {
            let find = bisect(
                prev_x,
                test_x,
                |x| {
                    if x == point.0 {
                        upper_start
                    } else {
                        get_slope(x, f(x) + epsilon) - d(x)
                    }
                },
                TOL,
            );
            let find_dir = get_slope(find, f(find) + epsilon); // d(find);
            if find_dir <= low_upper {
                new_low_upper = find_dir;
                new_upper_point = find;
                found_upper = true;
            }
        }

        if lower_slope >= high_lower {
            new_high_lower = lower_slope;
            new_lower_point = test_x;
        }

        if lower_rdir < 0.0 && new_lower_rdir > 0.0 {
            let find = bisect(
                prev_x,
                test_x,
                |x| {
                    if x == point.0 {
                        lower_start
                    } else {
                        get_slope(x, f(x) - epsilon) - d(x)
                    }
                },
                TOL,
            );
            let find_dir = get_slope(find, f(find) - epsilon); // d(find);
            if find_dir >= high_lower {
                new_high_lower = find_dir;
                new_lower_point = find;
                found_lower = true;
            }
        }

        if found_upper {
            if new_low_upper < high_lower {
                upper_point = new_upper_point;
                hit_upper = true;
                break;
            }
        }

        if found_lower {
            if low_upper < new_high_lower {
                lower_point = new_lower_point;
                hit_upper = false;
                break;
            }
        }

        low_upper = new_low_upper;
        high_lower = new_high_lower;
        lower_point = new_lower_point;
        upper_point = new_upper_point;

        // when this occurs, found_lower and found_upper are both true
        if low_upper < high_lower {
            hit_upper = upper_point > lower_point;
            break;
        }

        if test_x == max_x {
            return Err((high_lower, low_upper));
        }

        upper_rdir = new_upper_rdir;
        lower_rdir = new_lower_rdir;
        prev_dd = new_dd;
    }

    if hit_upper {
        let find_x = find_root(
            prev_x,
            upper_point,
            None,
            |x| get_slope(x, f(x) + epsilon) - high_lower,
            |x| {
                let y = f(x) + epsilon;
                let bottom = x - point.0;
                (bottom * d(x) - (y - point.1)) / (bottom * bottom)
            },
            TOL,
        );
        return Ok((high_lower, find_x, lower_point, true));
    } else {
        let find_x = find_root(
            prev_x,
            lower_point,
            None,
            |x| get_slope(x, f(x) - epsilon) - low_upper,
            |x| {
                let y = f(x) - epsilon;
                let bottom = x - point.0;
                (bottom * d(x) - (y - point.1)) / (bottom * bottom)
            },
            TOL,
        );

        return Ok((low_upper, find_x, upper_point, false));
    }
}

// a struct to help with directional first and second derivatives
#[derive(Clone, Copy, Debug)]
pub struct Line {
    x: f64,
    y: f64,
    dir_x: f64,
    dir_y: f64,
}

impl Line {
    #[inline]
    fn pos(self, t: f64) -> (f64, f64) {
        (self.x + t * self.dir_x, self.y + t * self.dir_y)
    }

    #[inline]
    fn val<F: Fn(f64, f64) -> f64>(self, t: f64, f: F) -> f64 {
        let x = self.x + t * self.dir_x;
        let y = self.y + t * self.dir_y;
        f(x, y)
    }

    #[inline]
    fn der<D: Fn(f64, f64) -> [f64; 2]>(self, t: f64, d: D) -> f64 {
        let x = self.x + t * self.dir_x;
        let y = self.y + t * self.dir_y;
        let [dfdx, dfdy] = d(x, y);
        self.dir_x * dfdx + self.dir_y * dfdy
    }

    #[inline]
    fn der_2<D2: Fn(f64, f64) -> [f64; 3]>(self, t: f64, dd: D2) -> f64 {
        let x = self.x + t * self.dir_x;
        let y = self.y + t * self.dir_y;
        let [dxdx, dydy, dxdy] = dd(x, y);
        let x_part = self.dir_x * dxdx + self.dir_y * dxdy;
        let y_part = self.dir_x * dxdy + self.dir_y * dydy;
        self.dir_x * x_part + self.dir_y * y_part
    }
}

fn bisect<F: Fn(f64) -> f64>(mut start: f64, mut end: f64, f: F, tolerance: f64) -> f64 {
    let mut f_start = f(start);
    let f_end = f(end);
    if f_start == 0.0 {
        return f_start;
    }
    if f_end == 0.0 {
        return f_end;
    }
    while end - start > tolerance {
        let mid = (start + end) / 2.0;
        if mid == start || mid == end {
            return mid;
        }
        let f_mid = f(mid);
        if f_mid == 0.0 {
            return mid;
        }
        if (f_mid > 0.0) != (f_start > 0.0) {
            end = mid;
        } else {
            start = mid;
            f_start = f_mid;
        }
    }
    return (start + end) / 2.0;
}

// attempts to use newton's method, but switches to bisection
// if it doesn't converge (or converge to the right root).
fn find_root<F: Fn(f64) -> f64, D: Fn(f64) -> f64>(
    mut start: f64,
    mut end: f64,
    mut initial: Option<f64>,
    f: F,
    d: D,
    tolerance: f64,
) -> f64 {
    // every iteration of this loop causes the range to get smaller
    // so it will eventually be smaller than tol
    // (assuming the tolerance is positive and numerical coarseness doesn't
    // cause problems)
    loop {
        let mut x = if let Some(x) = initial {
            x
        } else {
            (start + end) / 2.0
        };
        let mut n = 0;
        // newton's method
        // in theory this could get stuck in a cycle, so
        // we need a condition to prevent the loop from going
        // infinite. In most cases the break will terminate
        // this loop before the while condition does
        while n < 8 {
            n += 1;
            let offset = f(x) / d(x);
            if offset.abs() < tolerance {
                return x - offset;
            } else {
                x = x - offset;
            }
            // < and >= are not exact negations of eachother
            // because of NaN. This expression will trigger
            // if x is NaN, which makes this more stable.
            // this condition will also work if d(x) is 0.0,
            // which will cause offset to be +-infinity
            if !(end < x && x < start) {
                break;
            }
        }
        let mid = (start + end) / 2.0;
        let f_mid = f(mid);
        // bisection
        if mid == start || mid == end {
            return mid;
        }
        if (f_mid < 0.0) != (f(end) < 0.0) {
            start = mid;
        } else {
            end = mid;
        }
        initial = None;
        if (end - start) < tolerance {
            return mid;
        }
    }
}