#include <iostream>
#include <sstream>
#include "machine.h"

grbl::machine::machine() {
    pipe = new tcp_transport("192.168.5.39", 23);
}

grbl::machine::~machine() {
    delete pipe;
}

void grbl::machine::connect() {
    pipe->open(*this);
}

void grbl::machine::run_program(grbl::program pgm) {
    std::cout << "running program with " << pgm.number_of_instructions() << " instructions" << std::endl;
    running_program = pgm;
    state = grbl_machine_state::run_program;

    executed_instructions = 0;
    continue_program();
}

void grbl::machine::on_connected(grbl::transport *transport) {
    std::cout << "grbl machine connected" << std::endl;
    // telnet handshake so that we get the banner. banner won't be coming otherwise
//        t->send("\xff\xfd\x18\xff\xfd\x20\xff\xfd\x23\xff\xfd\x27");
    pipe->send("\xff\xfd\x18");

}

void grbl::machine::on_disconnected(grbl::transport *transport) {
    std::cout << "grbl machine disconnected" << std::endl;
}

void grbl::machine::on_line_received(std::string line, grbl::transport *transport) {
    std::cout << ">> " << line << std::endl;

    if (state == grbl_machine_state::run_program) {
        if (line.rfind("ok", 0) == 0) {
            continue_program();
        } else if (line.rfind("error", 0) == 0) {
            std::cerr << "Received error" << std::endl;
        } else {
        }
    } else {
        // evaluate responses when not running a program
    }
}

void grbl::machine::on_banner(std::string version, grbl::transport *transport) {
    std::cout << "grbl machine received banner" << std::endl;
}

void grbl::machine::continue_program() {
    if (executed_instructions < running_program.number_of_instructions()) {
        instruction to_send;
        do {
            to_send = running_program.instruction_at(executed_instructions++);
        } while (to_send.type != instruction_type::gcode && executed_instructions < running_program.number_of_instructions());

        if (to_send.type == instruction_type::gcode) {
            pipe->send(to_send.command);
        } else {
            state = grbl_machine_state::idle;
        }
    } else {
        state = grbl_machine_state::idle;
    }
}

void grbl::machine::request_jog(jog_state jog) const {
    cancel_jog();
    if (jog.no_jogging()) {
//        cancel_jog();
        return;
    }

    std::stringstream ss;
    ss << "$J=G91 G21 ";

    if (jog.left_pressed)
        ss << " X-1000";
    else if (jog.right_pressed)
        ss << " X1000";

    if (jog.up_pressed)
        ss << " Y1000";
    else if (jog.down_pressed)
        ss << " Y-1000";

    if (jog.z_up_pressed)
        ss << " Z1000";
    else if (jog.z_down_pressed)
        ss << " Z-1000";

    if (jog.speed_fast_pressed) {
        ss << " F10000";
    } else if (jog.speed_slow_pressed) {
        ss << " F100";
    } else {
        ss << " F1000";
    }

    pipe->send(ss.str());
}

void grbl::machine::cancel_jog() const {
    pipe->send_single_char_command(0x85);


}

bool grbl::jog_state::no_jogging() const {
    return !(up_pressed || down_pressed || left_pressed || right_pressed || z_up_pressed || z_down_pressed);
}