import json
import math
import sys

import FreeCAD as App
import Part
import Sketcher


TOLERANCE = 1e-7


def line(start_x, start_y, end_x, end_y):
    return Part.LineSegment(App.Vector(start_x, start_y, 0), App.Vector(end_x, end_y, 0))


def point(x, y):
    return Part.Point(App.Vector(x, y, 0))


def circle(center_x, center_y, radius):
    return Part.Circle(App.Vector(center_x, center_y, 0), App.Vector(0, 0, 1), radius)


def distance(first, second):
    return math.hypot(first.x - second.x, first.y - second.y)


def line_length(geometry):
    return distance(geometry.StartPoint, geometry.EndPoint)


def constraint_types(sketch):
    return [str(constraint.Type) for constraint in sketch.Constraints]


def solver_state(sketch, solve_status):
    return {
        "solveStatus": int(solve_status),
        "degreesOfFreedom": int(sketch.DoF),
        "fullyConstrained": bool(sketch.FullyConstrained),
        "conflicting": [int(value) for value in sketch.ConflictingConstraints],
        "redundant": [int(value) for value in sketch.RedundantConstraints],
        "partiallyRedundant": [int(value) for value in sketch.PartiallyRedundantConstraints],
        "malformed": [int(value) for value in sketch.MalformedConstraints],
        "constraintTypes": constraint_types(sketch),
    }


def new_sketch(case_id):
    document = App.newDocument("Oracle_" + case_id.replace("-", "_"))
    sketch = document.addObject("Sketcher::SketchObject", "Sketch")
    sketch.MapMode = "Deactivated"
    return document, sketch


def close_document(document):
    App.closeDocument(document.Name)


def run_success(case_id, constraint_type, setup):
    document, sketch = new_sketch(case_id)
    try:
        residual = setup(sketch)
        solve_status = sketch.solve()
        document.recompute()
        state = solver_state(sketch, solve_status)
        measured_residual = float(residual(sketch))
        passed = (
            state["solveStatus"] == 0
            and not state["conflicting"]
            and not state["redundant"]
            and not state["malformed"]
            and math.isfinite(measured_residual)
            and abs(measured_residual) <= TOLERANCE
        )
        return {
            "id": case_id,
            "constraintType": constraint_type,
            "expected": "success",
            "observed": "success" if passed else "unexpected-failure",
            "residual": measured_residual,
            **state,
        }
    except Exception as error:
        return {
            "id": case_id,
            "constraintType": constraint_type,
            "expected": "success",
            "observed": "exception",
            "errorType": type(error).__name__,
            "error": str(error),
        }
    finally:
        close_document(document)


def add_lines(sketch):
    return sketch.addGeometry([
        line(0, 0, 4, 0),
        line(0, 2, 3, 6),
    ], False)


def setup_coincident(sketch):
    add_lines(sketch)
    sketch.addConstraint(Sketcher.Constraint("Coincident", 0, 2, 1, 1))
    return lambda value: distance(value.Geometry[0].EndPoint, value.Geometry[1].StartPoint)


def setup_horizontal(sketch):
    sketch.addGeometry(line(0, 0, 3, 4), False)
    sketch.addConstraint(Sketcher.Constraint("Horizontal", 0))
    return lambda value: value.Geometry[0].EndPoint.y - value.Geometry[0].StartPoint.y


def setup_vertical(sketch):
    sketch.addGeometry(line(0, 0, 3, 4), False)
    sketch.addConstraint(Sketcher.Constraint("Vertical", 0))
    return lambda value: value.Geometry[0].EndPoint.x - value.Geometry[0].StartPoint.x


def setup_parallel(sketch):
    add_lines(sketch)
    sketch.addConstraint(Sketcher.Constraint("Parallel", 0, 1))
    return lambda value: (
        (value.Geometry[0].EndPoint.x - value.Geometry[0].StartPoint.x)
        * (value.Geometry[1].EndPoint.y - value.Geometry[1].StartPoint.y)
        - (value.Geometry[0].EndPoint.y - value.Geometry[0].StartPoint.y)
        * (value.Geometry[1].EndPoint.x - value.Geometry[1].StartPoint.x)
    ) / (line_length(value.Geometry[0]) * line_length(value.Geometry[1]))


def setup_perpendicular(sketch):
    add_lines(sketch)
    sketch.addConstraint(Sketcher.Constraint("Perpendicular", 0, 1))
    return lambda value: (
        (value.Geometry[0].EndPoint.x - value.Geometry[0].StartPoint.x)
        * (value.Geometry[1].EndPoint.x - value.Geometry[1].StartPoint.x)
        + (value.Geometry[0].EndPoint.y - value.Geometry[0].StartPoint.y)
        * (value.Geometry[1].EndPoint.y - value.Geometry[1].StartPoint.y)
    ) / (line_length(value.Geometry[0]) * line_length(value.Geometry[1]))


def setup_equal(sketch):
    add_lines(sketch)
    sketch.addConstraint(Sketcher.Constraint("Equal", 0, 1))
    return lambda value: line_length(value.Geometry[0]) - line_length(value.Geometry[1])


def setup_tangent(sketch):
    sketch.addGeometry([circle(0, 0, 2), circle(6, 0, 1)], False)
    sketch.addConstraint(Sketcher.Constraint("Tangent", 0, 1))
    return lambda value: distance(value.Geometry[0].Center, value.Geometry[1].Center) - value.Geometry[0].Radius - value.Geometry[1].Radius


def setup_distance(sketch):
    sketch.addGeometry(line(0, 0, 3, 4), False)
    sketch.addConstraint(Sketcher.Constraint("Distance", 0, 5.0))
    return lambda value: line_length(value.Geometry[0]) - 5.0


def setup_distance_x(sketch):
    sketch.addGeometry(line(0, 0, 3, 4), False)
    sketch.addConstraint(Sketcher.Constraint("DistanceX", 0, 1, 0, 2, 3.0))
    return lambda value: value.Geometry[0].EndPoint.x - value.Geometry[0].StartPoint.x - 3.0


def setup_distance_y(sketch):
    sketch.addGeometry(line(0, 0, 3, 4), False)
    sketch.addConstraint(Sketcher.Constraint("DistanceY", 0, 1, 0, 2, 4.0))
    return lambda value: value.Geometry[0].EndPoint.y - value.Geometry[0].StartPoint.y - 4.0


def setup_angle(sketch):
    sketch.addGeometry(line(0, 0, 3, 3), False)
    sketch.addConstraint(Sketcher.Constraint("Angle", 0, math.pi / 4))
    return lambda value: math.atan2(
        value.Geometry[0].EndPoint.y - value.Geometry[0].StartPoint.y,
        value.Geometry[0].EndPoint.x - value.Geometry[0].StartPoint.x,
    ) - math.pi / 4


def setup_radius(sketch):
    sketch.addGeometry(circle(2, 3, 4), False)
    sketch.addConstraint(Sketcher.Constraint("Radius", 0, 4.0))
    return lambda value: value.Geometry[0].Radius - 4.0


def setup_diameter(sketch):
    sketch.addGeometry(circle(2, 3, 4), False)
    sketch.addConstraint(Sketcher.Constraint("Diameter", 0, 8.0))
    return lambda value: 2 * value.Geometry[0].Radius - 8.0


def setup_point_on_object(sketch):
    sketch.addGeometry([point(2, 3), line(0, 0, 4, 0)], False)
    sketch.addConstraint(Sketcher.Constraint("PointOnObject", 0, 1, 1))
    return lambda value: value.Geometry[0].Y - value.Geometry[1].StartPoint.y


def setup_symmetric(sketch):
    sketch.addGeometry([point(1, 2), point(0, 0), point(4, 6)], False)
    sketch.addConstraint(Sketcher.Constraint("Symmetric", 0, 1, 1, 1, 2, 1))
    return lambda value: math.hypot(
        (value.Geometry[0].X + value.Geometry[1].X) / 2 - value.Geometry[2].X,
        (value.Geometry[0].Y + value.Geometry[1].Y) / 2 - value.Geometry[2].Y,
    )


def setup_internal_alignment(sketch):
    sketch.addGeometry(Part.Ellipse(App.Vector(2, 3, 0), 5, 3), False)
    sketch.exposeInternalGeometry(0)
    return lambda value: 0 if "InternalAlignment" in constraint_types(value) else 1


def setup_snells_law(sketch):
    sketch.addGeometry([
        line(0, 0, -4, 4),
        line(0, 0, 4, 3),
        line(-5, 0, 5, 0),
    ], False)
    sketch.addConstraint(Sketcher.Constraint("Coincident", 0, 1, 1, 1))
    sketch.addConstraint(Sketcher.Constraint("PointOnObject", 0, 1, 2))
    sketch.addConstraint(Sketcher.Constraint("SnellsLaw", 0, 1, 1, 1, 2, 1.5))
    return lambda value: 0 if "SnellsLaw" in constraint_types(value) else 1


def setup_block(sketch):
    sketch.addGeometry(line(1, 2, 4, 6), False)
    sketch.addConstraint(Sketcher.Constraint("Block", 0))
    return lambda value: distance(value.Geometry[0].StartPoint, App.Vector(1, 2, 0)) + distance(value.Geometry[0].EndPoint, App.Vector(4, 6, 0))


def setup_weight(sketch):
    curve = Part.BSplineCurve()
    curve.buildFromPolesMultsKnots(
        [App.Vector(0, 0, 0), App.Vector(1, 2, 0), App.Vector(3, 2, 0), App.Vector(4, 0, 0)],
        [4, 4],
        [0, 1],
        False,
        3,
        [1, 0.75, 1.25, 1],
    )
    sketch.addGeometry(curve, False)
    sketch.exposeInternalGeometry(0)
    weight_index = next(index for index, constraint in enumerate(sketch.Constraints) if str(constraint.Type) == "Weight")
    sketch.setDatum(weight_index, App.Units.Quantity("1.5"))
    return lambda value: value.Geometry[0].getWeights()[0] - 1.5


SUCCESS_SETUPS = [
    ("coincident-success", "Coincident", setup_coincident),
    ("horizontal-success", "Horizontal", setup_horizontal),
    ("vertical-success", "Vertical", setup_vertical),
    ("parallel-success", "Parallel", setup_parallel),
    ("tangent-success", "Tangent", setup_tangent),
    ("distance-success", "Distance", setup_distance),
    ("distance-x-success", "DistanceX", setup_distance_x),
    ("distance-y-success", "DistanceY", setup_distance_y),
    ("angle-success", "Angle", setup_angle),
    ("perpendicular-success", "Perpendicular", setup_perpendicular),
    ("radius-success", "Radius", setup_radius),
    ("equal-success", "Equal", setup_equal),
    ("point-on-object-success", "PointOnObject", setup_point_on_object),
    ("symmetric-success", "Symmetric", setup_symmetric),
    ("internal-alignment-success", "InternalAlignment", setup_internal_alignment),
    ("snells-law-success", "SnellsLaw", setup_snells_law),
    ("block-success", "Block", setup_block),
    ("diameter-success", "Diameter", setup_diameter),
    ("weight-success", "Weight", setup_weight),
]


def base_failure_geometry(sketch):
    sketch.addGeometry([
        line(0, 0, 4, 0),
        line(0, 2, 3, 6),
        point(2, 3),
        circle(8, 3, 2),
        Part.Ellipse(App.Vector(2, 3, 0), 5, 3),
    ], False)


FAILURE_CONSTRAINTS = [
    ("coincident-invalid-reference", "Coincident", lambda: Sketcher.Constraint("Coincident", 0, 2, 99, 1)),
    ("horizontal-invalid-reference", "Horizontal", lambda: Sketcher.Constraint("Horizontal", 99)),
    ("vertical-invalid-reference", "Vertical", lambda: Sketcher.Constraint("Vertical", 99)),
    ("parallel-invalid-reference", "Parallel", lambda: Sketcher.Constraint("Parallel", 0, 99)),
    ("tangent-invalid-reference", "Tangent", lambda: Sketcher.Constraint("Tangent", 3, 99)),
    ("distance-invalid-reference", "Distance", lambda: Sketcher.Constraint("Distance", 99, 5.0)),
    ("distance-x-invalid-reference", "DistanceX", lambda: Sketcher.Constraint("DistanceX", 99, 1, 0, 2, 3.0)),
    ("distance-y-invalid-reference", "DistanceY", lambda: Sketcher.Constraint("DistanceY", 99, 1, 0, 2, 4.0)),
    ("angle-invalid-reference", "Angle", lambda: Sketcher.Constraint("Angle", 99, math.pi / 4)),
    ("perpendicular-invalid-reference", "Perpendicular", lambda: Sketcher.Constraint("Perpendicular", 0, 99)),
    ("radius-invalid-reference", "Radius", lambda: Sketcher.Constraint("Radius", 99, 4.0)),
    ("equal-invalid-reference", "Equal", lambda: Sketcher.Constraint("Equal", 0, 99)),
    ("point-on-object-invalid-reference", "PointOnObject", lambda: Sketcher.Constraint("PointOnObject", 2, 1, 99)),
    ("symmetric-invalid-reference", "Symmetric", lambda: Sketcher.Constraint("Symmetric", 2, 1, 2, 1, 99, 1)),
    ("internal-alignment-invalid-reference", "InternalAlignment", lambda: Sketcher.Constraint("InternalAlignment:EllipseMajorDiameter", 99, 4)),
    ("snells-law-invalid-reference", "SnellsLaw", lambda: Sketcher.Constraint("SnellsLaw", 0, 1, 1, 1, 99, 1.5)),
    ("block-invalid-reference", "Block", lambda: Sketcher.Constraint("Block", 99)),
    ("diameter-invalid-reference", "Diameter", lambda: Sketcher.Constraint("Diameter", 99, 8.0)),
    ("weight-invalid-reference", "Weight", lambda: Sketcher.Constraint("Weight", 99, 1.5)),
]


def run_failure(case_id, constraint_type, constraint_factory):
    document, sketch = new_sketch(case_id)
    try:
        base_failure_geometry(sketch)
        add_result = None
        error = None
        try:
            add_result = int(sketch.addConstraint(constraint_factory()))
        except Exception as caught:
            error = caught
        solve_status = None
        state = None
        if error is None:
            try:
                solve_status = int(sketch.solve())
                state = solver_state(sketch, solve_status)
            except Exception as caught:
                error = caught
        failure_observed = (
            error is not None
            or add_result is None
            or add_result < 0
            or solve_status is None
            or solve_status < 0
            or bool(state and (state["conflicting"] or state["redundant"] or state["malformed"]))
        )
        return {
            "id": case_id,
            "constraintType": constraint_type,
            "expected": "failure",
            "observed": "failure" if failure_observed else "unexpected-success",
            "addResult": add_result,
            **(state or {}),
            **({"errorType": type(error).__name__, "error": str(error)} if error else {}),
        }
    finally:
        close_document(document)


def run_classification_cases():
    cases = []

    document, sketch = new_sketch("classification-reference")
    try:
        sketch.addGeometry(line(0, 0, 2, 1), False)
        sketch.addConstraint(Sketcher.Constraint("Horizontal", 0))
        reference_index = sketch.addConstraint([Sketcher.Constraint("Distance", 0, 99.0)])[0]
        sketch.setDriving(reference_index, False)
        solve_status = sketch.solve()
        state = solver_state(sketch, solve_status)
        measured_value = float(sketch.Constraints[reference_index].Value)
        passed = state["solveStatus"] == 0 and state["degreesOfFreedom"] == 3 and not state["conflicting"] and not state["redundant"] and not state["malformed"] and sketch.Constraints[reference_index].Driving is False and math.isfinite(measured_value) and abs(measured_value - 99.0) > TOLERANCE
        cases.append({"id": "reference-dimension", "expected": "reference", "observed": "reference" if passed else "unexpected", "inputValue": 99.0, "measuredValue": measured_value, "driving": bool(sketch.Constraints[reference_index].Driving), **state})
    finally:
        close_document(document)

    for case_id, second_value, expected_status, expected_redundant, expected_conflicting in [
        ("redundant-dimension", 5.0, -2, [2], []),
        ("conflicting-dimension", 8.0, -3, [], [1, 2]),
    ]:
        document, sketch = new_sketch("classification-" + case_id)
        try:
            sketch.addGeometry(line(0, 0, 1, 0), False)
            sketch.addConstraint(Sketcher.Constraint("Distance", 0, 5.0))
            sketch.addConstraint(Sketcher.Constraint("Distance", 0, second_value))
            solve_status = sketch.solve()
            state = solver_state(sketch, solve_status)
            passed = state["solveStatus"] == expected_status and state["degreesOfFreedom"] == 3 and state["redundant"] == expected_redundant and state["conflicting"] == expected_conflicting and not state["malformed"]
            expected = "redundant" if expected_redundant else "conflicting"
            cases.append({"id": case_id, "expected": expected, "observed": expected if passed else "unexpected", "values": [5.0, second_value], **state})
        finally:
            close_document(document)
    return cases


success_cases = [run_success(*fixture) for fixture in SUCCESS_SETUPS]
failure_cases = [run_failure(*fixture) for fixture in FAILURE_CONSTRAINTS]
classification_cases = run_classification_cases()
version = App.Version()
report = {
    "schemaVersion": 1,
    "baselineId": "freecad-1.1.1-sketcher-constraint-oracle",
    "freecadVersion": ".".join(version[:3]),
    "revision": str(version[3]),
    "gitCommit": str(version[7]) if len(version) > 7 else "",
    "tolerance": TOLERANCE,
    "status": "pass" if all(case["observed"] == case["expected"] for case in success_cases + failure_cases + classification_cases) else "failed",
    "summary": {
        "constraintTypes": len(SUCCESS_SETUPS),
        "successCases": len(success_cases),
        "successPassed": sum(case["observed"] == "success" for case in success_cases),
        "failureCases": len(failure_cases),
        "failurePassed": sum(case["observed"] == "failure" for case in failure_cases),
        "classificationCases": len(classification_cases),
        "classificationPassed": sum(case["observed"] == case["expected"] for case in classification_cases),
    },
    "successCases": success_cases,
    "failureCases": failure_cases,
    "classificationCases": classification_cases,
}
print("FREECAD_SKETCHER_ORACLE_RESULT=" + json.dumps(report, sort_keys=True, separators=(",", ":")))
sys.stdout.flush()
sys.exit(0)
