#include "sim_interpreter.h"

#include <assert.h>
#include <math.h>
#include <string.h>

int main(void) {
    sim_interpreter interpreter = {0};
    sim_interpreter parameter_interpreter = {0};
    sim_interpreter_state state = {0};
    sim_interpreter_state second = {0};
    sim_posemath_segment motion = {0};
    sim_posemath_pose point = {0};
    char canonical[SIM_INTERPRETER_CANONICAL_CAPACITY] = {0};
    uint32_t required;

    assert(sim_interpreter_init(&interpreter) == SIM_INTERP_OK);
    assert(sim_interpreter_execute(&interpreter, "G21 G90 G1 X10 Y2 F120", (uint32_t)(sizeof("G21 G90 G1 X10 Y2 F120") - 1)) == SIM_INTERP_OK);
    assert(sim_interpreter_read_state(&interpreter, &state) == SIM_INTERP_OK);
    assert(state.x == 10.0 && state.y == 2.0 && state.feed == 120.0);
    assert(sim_interpreter_read_motion(&interpreter, &motion) == SIM_INTERP_OK);
    assert(fabs(motion.velocity - 2.0) < 1e-12);
    required = sim_interpreter_canonical(&interpreter, canonical, sizeof(canonical));
    assert(required > 1 && canonical[required - 1] == '\0');
    assert(sim_interpreter_state_hash(&interpreter) != 0);
    assert(sim_interpreter_modal_hash(&interpreter) != 0);
    assert(sim_interpreter_size() == sizeof(sim_interpreter));
    assert(sim_interpreter_init(&parameter_interpreter) == SIM_INTERP_OK);
    assert(sim_interpreter_execute(&parameter_interpreter, "#1=10 #2=[#1*2+5] X[#2/5]", (uint32_t)(sizeof("#1=10 #2=[#1*2+5] X[#2/5]") - 1)) == SIM_INTERP_OK);
    assert(fabs(parameter_interpreter.state.x - 5.0) < 1e-12);
    {
        double parameter = 0.0;
    assert(sim_interpreter_parameter_get(&parameter_interpreter, 2, &parameter) == SIM_INTERP_OK);
        assert(fabs(parameter - 25.0) < 1e-12);
    }
    assert(sim_interpreter_parameter_set(&parameter_interpreter, 5399, 12.5) == SIM_INTERP_OK);
    assert(sim_interpreter_execute(&parameter_interpreter, "#100=[#5399+1] X[#100]", (uint32_t)(sizeof("#100=[#5399+1] X[#100]") - 1)) == SIM_INTERP_OK);
    assert(fabs(parameter_interpreter.state.x - 13.5) < 1e-12);
    assert(sim_interpreter_parameter_set(&parameter_interpreter, 5400, 1.0) == SIM_INTERP_ERR_PARAMETER);
    assert(sim_interpreter_execute(&parameter_interpreter, "G1 X?", 5) == SIM_INTERP_ERR_SYNTAX);
    assert(sim_interpreter_error_column(&parameter_interpreter) == 4);
    assert(sim_interpreter_error_code(&parameter_interpreter) == SIM_INTERP_ERR_SYNTAX);
    assert(sim_interpreter_execute(&interpreter, "G91 X1 (increment)", (uint32_t)(sizeof("G91 X1 (increment)") - 1)) == SIM_INTERP_OK);
    assert(sim_interpreter_read_state(&interpreter, &second) == SIM_INTERP_OK);
    assert(second.x == 11.0 && second.line == state.line + 1);
    assert(sim_interpreter_execute(&interpreter, "G17 G02 I1 J1", (uint32_t)(sizeof("G17 G02 I1 J1") - 1)) == SIM_INTERP_OK);
    assert(sim_interpreter_read_motion(&interpreter, &motion) == SIM_INTERP_OK);
    assert(motion.motion_type == SIM_POSEMATH_CIRCULAR && motion.turn < 0);
    assert(sim_posemath_segment_point(&motion, 0.5, &point) == SIM_POSEMATH_OK);
    assert(sim_interpreter_execute(&interpreter, "G20 X1", 6) == SIM_INTERP_OK);
    assert(fabs(interpreter.state.x - 36.4) < 1e-12);
    assert(sim_interpreter_execute(&interpreter, "X1 G90 G21", (uint32_t)(sizeof("X1 G90 G21") - 1)) == SIM_INTERP_OK);
    assert(interpreter.state.x == 1.0);
    assert(sim_interpreter_execute(&interpreter, "G1 G0 X1", 8) == SIM_INTERP_ERR_STATE);
    assert(sim_interpreter_execute(&interpreter, "G999", 4) == SIM_INTERP_ERR_UNKNOWN_CODE);
    assert(sim_interpreter_execute(&interpreter, "Xnan", 4) == SIM_INTERP_ERR_SYNTAX);
    assert(sim_interpreter_execute(&interpreter, "G1 X1 X2", 8) == SIM_INTERP_ERR_STATE);
    assert(sim_interpreter_execute(&interpreter, "M2", 2) == SIM_INTERP_OK);
    assert(interpreter.state.program_end == 1);
    assert(sim_interpreter_execute(&interpreter, "G1 X0", (uint32_t)(sizeof("G1 X0") - 1)) == SIM_INTERP_ERR_STATE);
    assert(sim_interpreter_reset(&interpreter) == SIM_INTERP_OK);
    assert(sim_interpreter_execute(&interpreter, "G10 L2 P1 X10 Y20 Z30", (uint32_t)(sizeof("G10 L2 P1 X10 Y20 Z30") - 1)) == SIM_INTERP_OK);
    assert(sim_interpreter_execute(&interpreter, "G54 G90 X1 Y2 Z3", (uint32_t)(sizeof("G54 G90 X1 Y2 Z3") - 1)) == SIM_INTERP_OK);
    assert(fabs(interpreter.state.x - 11.0) < 1e-12);
    assert(fabs(interpreter.state.y - 22.0) < 1e-12);
    assert(fabs(interpreter.state.z - 33.0) < 1e-12);
    assert(sim_interpreter_execute(&interpreter, "G53 G0 X0 Y0 Z10", (uint32_t)(sizeof("G53 G0 X0 Y0 Z10") - 1)) == SIM_INTERP_OK);
    assert(fabs(interpreter.state.x) < 1e-12 && fabs(interpreter.state.y) < 1e-12 && fabs(interpreter.state.z - 10.0) < 1e-12);
    assert(sim_interpreter_execute(&interpreter, "G10 L20 P0 X0 Y0 Z10", (uint32_t)(sizeof("G10 L20 P0 X0 Y0 Z10") - 1)) == SIM_INTERP_OK);
    assert(sim_interpreter_read_motion(&interpreter, &motion) == SIM_INTERP_ERR_STATE);
    assert(sim_interpreter_execute(&interpreter, "G0 X20 Y20 Z10", (uint32_t)(sizeof("G0 X20 Y20 Z10") - 1)) == SIM_INTERP_OK);
    assert(sim_interpreter_execute(&interpreter, "G0 X[#<_x>-10]", (uint32_t)(sizeof("G0 X[#<_x>-10]") - 1)) == SIM_INTERP_OK);
    assert(fabs(interpreter.state.x - 10.0) < 1e-12 && fabs(interpreter.state.y - 20.0) < 1e-12);
    assert(sim_interpreter_reset(&interpreter) == SIM_INTERP_OK);
    assert(sim_interpreter_execute(&interpreter, "G54 G43 H12 G81 X1 Y2 Z-3 R1", (uint32_t)(sizeof("G54 G43 H12 G81 X1 Y2 Z-3 R1") - 1)) == SIM_INTERP_OK);
    assert(interpreter.state.motion_mode == 1);
    assert(sim_interpreter_modal_hash(&interpreter) != 0);
    assert(sim_interpreter_execute(&interpreter, "G80 M7", (uint32_t)(sizeof("G80 M7") - 1)) == SIM_INTERP_OK);
    assert(sim_interpreter_execute(&interpreter, "M8", 2) == SIM_INTERP_OK);
    assert(sim_interpreter_execute(&interpreter, "M9", 2) == SIM_INTERP_OK);
    return 0;
}