/*
      This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
      Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
      Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
      You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
*/

#include "libs/Kernel.h"
#include "libs/SerialMessage.h"
#include "Panel.h"
#include "PanelScreen.h"
#include "MainMenuScreen.h"
#include "ControlScreen.h"
#include "libs/nuts_bolts.h"
#include "libs/utils.h"
#include <string>
#include "Robot.h"
#include "PublicData.h"
#include "checksumm.h"
#include "LcdBase.h"

#include <math.h>
#include <stdio.h>

using namespace std;

#define NULL_CONTROL_MODE        0
#define AXIS_CONTROL_MODE        1

ControlScreen::ControlScreen()
{
    this->control_mode = NULL_CONTROL_MODE;
}

void ControlScreen::on_enter()
{
    THEPANEL->enter_menu_mode();
    THEPANEL->setup_menu(4);
    get_current_pos(this->pos); // gets current WCS
    this->refresh_menu();
    this->pos_changed = false;
}

// called in on_idle()
void ControlScreen::on_refresh()
{
    if ( THEPANEL->menu_change() ) {
        this->refresh_menu();
    }

    if (this->control_mode == AXIS_CONTROL_MODE) {

        if ( THEPANEL->click() ) {
            this->enter_menu_control();
            this->refresh_menu();

        } else if (THEPANEL->control_value_change()) {
            this->pos[this->controlled_axis - 'X'] = THEPANEL->get_control_value();
            THEPANEL->lcd->setCursor(0, 2);
            this->display_axis_line(this->controlled_axis);
            this->pos_changed = true; // make the gcode in main_loop
        }

    } else {
        if ( THEPANEL->click() ) {
            this->clicked_menu_entry(THEPANEL->get_menu_current_line());
        }
    }
}

// queuing gcodes needs to be done from main loop
void ControlScreen::on_main_loop()
{
    // change actual axis value
    if (!this->pos_changed) return;
    this->pos_changed = false;

    set_current_pos(this->controlled_axis, this->pos[this->controlled_axis - 'X']);
}

void ControlScreen::display_menu_line(uint16_t line)
{
    // in menu mode
    switch ( line ) {
        case 0: THEPANEL->lcd->printf("Back");  break;
        case 1: this->display_axis_line('X'); break;
        case 2: this->display_axis_line('Y'); break;
        case 3: this->display_axis_line('Z'); break;
    }
}

void ControlScreen::display_axis_line(char axis)
{
    THEPANEL->lcd->printf("Move %c    %8.3f", axis, this->pos[axis - 'X']);
}


void ControlScreen::clicked_menu_entry(uint16_t line)
{
    switch ( line ) {
        case 0: THEPANEL->enter_screen(this->parent   ); break;
        case 1: this->enter_axis_control('X'); break;
        case 2: this->enter_axis_control('Y'); break;
        case 3: this->enter_axis_control('Z'); break;
    }
}

void ControlScreen::enter_axis_control(char axis)
{
    this->control_mode = AXIS_CONTROL_MODE;
    this->controlled_axis = axis;
    THEPANEL->enter_control_mode(this->jog_increment, this->jog_increment / 10);
    THEPANEL->set_control_value(this->pos[axis - 'X']);
    THEPANEL->lcd->clear();
    THEPANEL->lcd->setCursor(0, 0);
    THEPANEL->lcd->printf("Jog %6.3f", jog_increment);

    THEPANEL->lcd->setCursor(0, 2);
    this->display_axis_line(this->controlled_axis);
}

void ControlScreen::enter_menu_control()
{
    this->control_mode = NULL_CONTROL_MODE;
    THEPANEL->enter_menu_mode();
}

void ControlScreen::set_current_pos(char axis, float p)
{
    // change pos by issuing a G0 Xnnn in absolute mode
    char buf[32];
    // make sure we are in absolute mode
    THEROBOT->push_state();
    THEROBOT->absolute_mode= true;
    int n = snprintf(buf, sizeof(buf), "G0 %c%f F%d", axis, p, (int)round(THEROBOT->from_millimeters(THEPANEL->get_jogging_speed(axis))));
    string g(buf, n);
    send_gcode(g);
    THEROBOT->pop_state();
}
