#include <memory.h>
#include <string.h>

#include "../libslic3r.h"
#include "../PrintConfig.hpp"

#include "PressureEqualizer.hpp"

namespace Slic3r {

GCodePressureEqualizer::GCodePressureEqualizer(const Slic3r::GCodeConfig *config) : 
    m_config(config)
{
    reset();
}

GCodePressureEqualizer::~GCodePressureEqualizer()
{
}

void GCodePressureEqualizer::reset()
{
    circular_buffer_pos     = 0;
    circular_buffer_size    = 100;
    circular_buffer_items   = 0;
    circular_buffer.assign(circular_buffer_size, GCodeLine());

    output_buffer.clear();
    output_buffer_length    = 0;

    m_current_extruder = 0;
    // Zero the position of the XYZE axes + the current feed
    memset(m_current_pos, 0, sizeof(float) * 5);
    m_current_extrusion_role = erNone;
    // Expect the first command to fill the nozzle (deretract).
    m_retracted = true;

    // Calculate filamet crossections for the multiple extruders.
    m_filament_crossections.clear();
    for (size_t i = 0; i < m_config->filament_diameter.values.size(); ++ i) {
        double r = m_config->filament_diameter.values[i];
        double a = 0.25f*M_PI*r*r;
        m_filament_crossections.push_back(float(a));
    }

    m_max_segment_length = 20.f;
    // Volumetric rate of a 0.45mm x 0.2mm extrusion at 60mm/s XY movement: 0.45*0.2*60*60=5.4*60 = 324 mm^3/min
    // Volumetric rate of a 0.45mm x 0.2mm extrusion at 20mm/s XY movement: 0.45*0.2*20*60=1.8*60 = 108 mm^3/min
    // Slope of the volumetric rate, changing from 20mm/s to 60mm/s over 2 seconds: (5.4-1.8)*60*60/2=60*60*1.8 = 6480 mm^3/min^2 = 1.8 mm^3/s^2
    m_max_volumetric_extrusion_rate_slope_positive = (this->m_config == NULL) ? 6480.f :
        this->m_config->max_volumetric_extrusion_rate_slope_positive.value * 60. * 60.;
    m_max_volumetric_extrusion_rate_slope_negative = (this->m_config == NULL) ? 6480.f :
        this->m_config->max_volumetric_extrusion_rate_slope_negative.value * 60. * 60.;

    for (size_t i = 0; i < numExtrusionRoles; ++ i) {
        m_max_volumetric_extrusion_rate_slopes[i].negative = m_max_volumetric_extrusion_rate_slope_negative;
        m_max_volumetric_extrusion_rate_slopes[i].positive = m_max_volumetric_extrusion_rate_slope_positive;
    }

    // Don't regulate the pressure in infill.
    m_max_volumetric_extrusion_rate_slopes[erBridgeInfill].negative = 0;
    m_max_volumetric_extrusion_rate_slopes[erBridgeInfill].positive = 0;
    // Don't regulate the pressure in gap fill.
    m_max_volumetric_extrusion_rate_slopes[erGapFill].negative = 0;
    m_max_volumetric_extrusion_rate_slopes[erGapFill].positive = 0;

    m_stat.reset();
}

const char* GCodePressureEqualizer::process(const char *szGCode, bool flush)
{
    // Reset length of the output_buffer.
    output_buffer_length = 0;

    if (szGCode != 0) {
        const char *p = szGCode;
        while (*p != 0) {
            // Find end of the line.
            const char *endl = p;
            // Slic3r always generates end of lines in a Unix style.
            for (; *endl != 0 && *endl != '\n'; ++ endl) ;
            if (circular_buffer_items == circular_buffer_size)
                // Buffer is full. Push out the oldest line.
                output_gcode_line(circular_buffer[circular_buffer_pos]);
            else
                ++ circular_buffer_items;
            // Process a G-code line, store it into the provided GCodeLine object.
            size_t idx_tail = circular_buffer_pos;
            circular_buffer_pos = circular_buffer_idx_next(circular_buffer_pos);
            if (! process_line(p, endl - p, circular_buffer[idx_tail])) {
                // The line has to be forgotten. It contains comment marks, which shall be
                // filtered out of the target g-code.
                circular_buffer_pos = idx_tail;
                -- circular_buffer_items;
            }
            p = endl;
            if (*p == '\n') 
                ++ p;
        }
    }

    if (flush) {
        // Flush the remaining valid lines of the circular buffer.
        for (size_t idx = circular_buffer_idx_head(); circular_buffer_items > 0; -- circular_buffer_items) {
            output_gcode_line(circular_buffer[idx]);
            if (++ idx == circular_buffer_size)
                idx = 0;
        }
        // Reset the index pointer.
        assert(circular_buffer_items == 0);
        circular_buffer_pos = 0;

#if 1 
        printf("Statistics: \n");
        printf("Minimum volumetric extrusion rate: %f\n", m_stat.volumetric_extrusion_rate_min);
        printf("Maximum volumetric extrusion rate: %f\n", m_stat.volumetric_extrusion_rate_max);
        if (m_stat.extrusion_length > 0)
            m_stat.volumetric_extrusion_rate_avg /= m_stat.extrusion_length;
        printf("Average volumetric extrusion rate: %f\n", m_stat.volumetric_extrusion_rate_avg);
        m_stat.reset();
#endif
    } 

    return output_buffer.data();
}

// Is a white space?
static inline bool is_ws(const char c) { return c == ' ' || c == '\t'; }
// Is it an end of line? Consider a comment to be an end of line as well.
static inline bool is_eol(const char c) { return c == 0 || c == '\r' || c == '\n' || c == ';'; };
// Is it a white space or end of line?
static inline bool is_ws_or_eol(const char c) { return is_ws(c) || is_eol(c); };

// Eat whitespaces.
static void eatws(const char *&line)
{
    while (is_ws(*line)) 
        ++ line;
}

// Parse an int starting at the current position of a line.
// If succeeded, the line pointer is advanced.
static inline int parse_int(const char *&line)
{
    char *endptr = NULL;
    long result = strtol(line, &endptr, 10);
    if (endptr == NULL || !is_ws_or_eol(*endptr))
        throw std::runtime_error("GCodePressureEqualizer: Error parsing an int");
    line = endptr;
    return int(result);
};

// Parse an int starting at the current position of a line.
// If succeeded, the line pointer is advanced.
static inline float parse_float(const char *&line)
{
    char *endptr = NULL;
    float result = strtof(line, &endptr);
    if (endptr == NULL || !is_ws_or_eol(*endptr))
        throw std::runtime_error("GCodePressureEqualizer: Error parsing a float");
    line = endptr;
    return result;
};

#define EXTRUSION_ROLE_TAG ";_EXTRUSION_ROLE:"
bool GCodePressureEqualizer::process_line(const char *line, const size_t len, GCodeLine &buf)
{
    if (strncmp(line, EXTRUSION_ROLE_TAG, strlen(EXTRUSION_ROLE_TAG)) == 0) {
        line += strlen(EXTRUSION_ROLE_TAG);
        int role = atoi(line);
        this->m_current_extrusion_role = ExtrusionRole(role);
        return false;
    }

    // Set the type, copy the line to the buffer.
    buf.type = GCODELINETYPE_OTHER;
    buf.modified = false;
    if (buf.raw.size() < len + 1)
        buf.raw.assign(line, line + len + 1);
    else
        memcpy(buf.raw.data(), line, len);
    buf.raw[len] = 0;
    buf.raw_length = len;

    memcpy(buf.pos_start, m_current_pos, sizeof(float)*5);
    memcpy(buf.pos_end, m_current_pos, sizeof(float)*5);
    memset(buf.pos_provided, 0, 5);

    buf.volumetric_extrusion_rate = 0.f;
    buf.volumetric_extrusion_rate_start = 0.f;
    buf.volumetric_extrusion_rate_end = 0.f;
    buf.max_volumetric_extrusion_rate_slope_positive = 0.f;
    buf.max_volumetric_extrusion_rate_slope_negative = 0.f;
	buf.extrusion_role = m_current_extrusion_role;

    // Parse the G-code line, store the result into the buf.
    switch (toupper(*line ++)) {
    case 'G': {
        int gcode = parse_int(line);
        eatws(line);
        switch (gcode) {
        case 0:
        case 1:
        {
            // G0, G1: A FFF 3D printer does not make a difference between the two.
            float new_pos[5];
            memcpy(new_pos, m_current_pos, sizeof(float)*5);
            bool  changed[5] = { false, false, false, false, false };
            while (!is_eol(*line)) {
                char axis = toupper(*line++);
                int  i = -1;
                switch (axis) {
                case 'X':
                case 'Y':
                case 'Z':
                    i = axis - 'X';
                    break;
                case 'E':
                    i = 3;
                    break;
                case 'F':
                    i = 4;
                    break;
                default:
                    assert(false);
                }
                if (i == -1)
                    throw std::runtime_error(std::string("GCodePressureEqualizer: Invalid axis for G0/G1: ") + axis);
                buf.pos_provided[i] = true;
                new_pos[i] = parse_float(line);
                if (i == 3 && m_config->use_relative_e_distances.value)
                    new_pos[i] += m_current_pos[i];
                changed[i] = new_pos[i] != m_current_pos[i];
                eatws(line);
            }
            if (changed[3]) {
                // Extrusion, retract or unretract.
                float diff = new_pos[3] - m_current_pos[3];
                if (diff < 0) {
                    buf.type = GCODELINETYPE_RETRACT;
                    m_retracted = true;
                } else if (! changed[0] && ! changed[1] && ! changed[2]) {
                    // assert(m_retracted);
                    buf.type = GCODELINETYPE_UNRETRACT;
                    m_retracted = false;
                } else {
                    assert(changed[0] || changed[1]);
                    // Moving in XY plane.
                    buf.type = GCODELINETYPE_EXTRUDE;
                    // Calculate the volumetric extrusion rate.
                    float diff[4];
                    for (size_t i = 0; i < 4; ++ i)
                        diff[i] = new_pos[i] - m_current_pos[i];
                    // volumetric extrusion rate = A_filament * F_xyz * L_e / L_xyz [mm^3/min]
                    float len2 = diff[0]*diff[0]+diff[1]*diff[1]+diff[2]*diff[2];
                    float rate = m_filament_crossections[m_current_extruder] * new_pos[4] * sqrt((diff[3]*diff[3])/len2);
                    buf.volumetric_extrusion_rate       = rate;
                    buf.volumetric_extrusion_rate_start = rate;
                    buf.volumetric_extrusion_rate_end   = rate;
                    m_stat.update(rate, sqrt(len2));
                    if (rate < 10.f) {
                    	printf("Extremely low flow rate: %f\n", rate);
                    }
                }
            } else if (changed[0] || changed[1] || changed[2]) {
                // Moving without extrusion.
                buf.type = GCODELINETYPE_MOVE;
            }
            memcpy(m_current_pos, new_pos, sizeof(float) * 5);
            break;
        }
        case 92:
        {
            // G92 : Set Position
            // Set a logical coordinate position to a new value without actually moving the machine motors.
            // Which axes to set?
            bool set = false;
            while (!is_eol(*line)) {
                char axis = toupper(*line++);
                switch (axis) {
                case 'X':
                case 'Y':
                case 'Z':
                    m_current_pos[axis - 'X'] = (!is_ws_or_eol(*line)) ? parse_float(line) : 0.f;
                    set = true;
                    break;
                case 'E':
                    m_current_pos[3] = (!is_ws_or_eol(*line)) ? parse_float(line) : 0.f;
                    set = true;
                    break;
                default:
                    throw std::runtime_error(std::string("GCodePressureEqualizer: Incorrect axis in a G92 G-code: ") + axis);
                }
                eatws(line);
            }
            assert(set);
            break;
        }
        case 10:
        case 22:
            // Firmware retract.
            buf.type = GCODELINETYPE_RETRACT;
            m_retracted = true;
            break;
        case 11:
        case 23:
            // Firmware unretract.
            buf.type = GCODELINETYPE_UNRETRACT;
            m_retracted = false;
            break;
        default:
            // Ignore the rest.
        break;
        }
        break;
    }
    case 'M': {
        int mcode = parse_int(line);
        eatws(line);
        switch (mcode) {
        default:
            // Ignore the rest of the M-codes.
        break;
        }
        break;
    }
    case 'T':
    {
        // Activate an extruder head.
        int new_extruder = parse_int(line);
        if (new_extruder != m_current_extruder) {
            m_current_extruder = new_extruder;
            m_retracted = true;
            buf.type = GCODELINETYPE_TOOL_CHANGE;
        } else {
            buf.type = GCODELINETYPE_NOOP;
        }
        break;
    }
    }

    buf.extruder_id = m_current_extruder;
    memcpy(buf.pos_end, m_current_pos, sizeof(float)*5);

    adjust_volumetric_rate();
	return true;
}

void GCodePressureEqualizer::output_gcode_line(GCodeLine &line)
{
    if (! line.modified) {
        push_to_output(line.raw.data(), line.raw_length, true);
        return;
    }

    // The line was modified.
    // Find the comment.
    const char *comment = line.raw.data();
    while (*comment != ';' && *comment != 0) ++comment;
    if (*comment != ';')
        comment = NULL;
    
    // Emit the line with lowered extrusion rates.
    float l2 = line.dist_xyz2();
    float l = sqrt(l2);
    size_t nSegments = size_t(ceil(l / m_max_segment_length));
    char text[2048];
    if (nSegments == 1) {
        // Just update this segment.
        push_line_to_output(line, line.feedrate() * line.volumetric_correction_avg(), comment);
    } else {
        bool accelerating = line.volumetric_extrusion_rate_start < line.volumetric_extrusion_rate_end;
        // Update the initial and final feed rate values.
        line.pos_start[4] = line.volumetric_extrusion_rate_start * line.pos_end[4] / line.volumetric_extrusion_rate;
        line.pos_end  [4] = line.volumetric_extrusion_rate_end   * line.pos_end[4] / line.volumetric_extrusion_rate;
        float feed_avg = 0.5f * (line.pos_start[4] + line.pos_end[4]);
        // Limiting volumetric extrusion rate slope for this segment.
        float max_volumetric_extrusion_rate_slope = accelerating ?
            line.max_volumetric_extrusion_rate_slope_positive : line.max_volumetric_extrusion_rate_slope_negative; 
        // Total time for the segment, corrected for the possibly lowered volumetric feed rate,
        // if accelerating / decelerating over the complete segment.
        float t_total = line.dist_xyz() / feed_avg;
        // Time of the acceleration / deceleration part of the segment, if accelerating / decelerating
        // with the maximum volumetric extrusion rate slope.
        float t_acc    = 0.5f * (line.volumetric_extrusion_rate_start + line.volumetric_extrusion_rate_end) / max_volumetric_extrusion_rate_slope;
        float l_acc    = l;
        float l_steady = 0.f;
        if (t_acc < t_total) {
            // One may achieve higher print speeds if part of the segment is not speed limited.
            float l_acc    = t_acc * feed_avg;
            float l_steady = l - l_acc;
            if (l_steady < 0.5f * m_max_segment_length) {
                l_acc    = l;
                l_steady = 0.f;
            } else
                nSegments = size_t(ceil(l_acc / m_max_segment_length));
        }
        float pos_start[5];
        float pos_end  [5];
        float pos_end2 [4];
        memcpy(pos_start, line.pos_start, sizeof(float)*5);
        memcpy(pos_end  , line.pos_end  , sizeof(float)*5);
        if (l_steady > 0.f) {
            // There will be a steady feed segment emitted.
            if (accelerating) {
                // Prepare the final steady feed rate segment.
                memcpy(pos_end2, pos_end, sizeof(float)*4);
                float t = l_acc / l;
                for (int i = 0; i < 4; ++ i) {
                    pos_end[i] = pos_start[i] + (pos_end[i] - pos_start[i]) * t;
                    line.pos_provided[i] = true;
                }
            } else {
                // Emit the steady feed rate segment.
                float t = l_steady / l;
                for (int i = 0; i < 4; ++ i) {
                    line.pos_end[i] = pos_start[i] + (pos_end[i] - pos_start[i]) * t;
                    line.pos_provided[i] = true;
                }
                push_line_to_output(line, pos_start[4], comment);
                comment = NULL;
                memcpy(line.pos_start, line.pos_end, sizeof(float)*5);
                memcpy(pos_start, line.pos_end, sizeof(float)*5);
            }
        }
        // Split the segment into pieces.
        for (size_t i = 1; i < nSegments; ++ i) {
            float t = float(i) / float(nSegments);
            for (size_t j = 0; j < 4; ++ j) {
                line.pos_end[j] = pos_start[j] + (pos_end[j] - pos_start[j]) * t;
                line.pos_provided[j] = true;
            } 
            // Interpolate the feed rate at the center of the segment.
            push_line_to_output(line, pos_start[4] + (pos_end[4] - pos_start[4]) * (float(i) - 0.5f) / float(nSegments), comment);
            comment = NULL;
            memcpy(line.pos_start, line.pos_end, sizeof(float)*5);
        }
		if (l_steady > 0.f && accelerating) {
            for (int i = 0; i < 4; ++ i) {
                line.pos_end[i] = pos_end2[i];
                line.pos_provided[i] = true;
            }
            push_line_to_output(line, pos_end[4], comment);
        }
    }
}

void GCodePressureEqualizer::adjust_volumetric_rate()
{
    if (circular_buffer_items < 2)
        return;

    // Go back from the current circular_buffer_pos and lower the feedtrate to decrease the slope of the extrusion rate changes.
    const size_t idx_head = circular_buffer_idx_head();
    const size_t idx_tail = circular_buffer_idx_prev(circular_buffer_idx_tail());
    size_t idx = idx_tail;
    if (idx == idx_head || ! circular_buffer[idx].extruding())
        // Nothing to do, the last move is not extruding.
        return;

    float feedrate_per_extrusion_role[numExtrusionRoles];
    for (size_t i = 0; i < numExtrusionRoles; ++ i)
        feedrate_per_extrusion_role[i] = FLT_MAX;
    feedrate_per_extrusion_role[circular_buffer[idx].extrusion_role] = circular_buffer[idx].volumetric_extrusion_rate_start;

    bool modified = true;
    while (modified && idx != idx_head) {
        size_t idx_prev = circular_buffer_idx_prev(idx);
        for (; ! circular_buffer[idx_prev].extruding() && idx_prev != idx_head; idx_prev = circular_buffer_idx_prev(idx_prev)) ;
        if (! circular_buffer[idx_prev].extruding())
        	break;
        float rate_succ = circular_buffer[idx].volumetric_extrusion_rate_start;
        // What is the gradient of the extrusion rate between idx_prev and idx?
        idx = idx_prev;
        GCodeLine &line = circular_buffer[idx];
        for (size_t iRole = 1; iRole < numExtrusionRoles; ++ iRole) {
            float rate_slope = m_max_volumetric_extrusion_rate_slopes[iRole].negative;
            if (rate_slope == 0)
                // The negative rate is unlimited.
                continue;
            float rate_end = feedrate_per_extrusion_role[iRole];
            if (iRole == line.extrusion_role && rate_succ < rate_end)
                rate_end = rate_succ;
            if (line.volumetric_extrusion_rate_end > rate_end) {
                line.volumetric_extrusion_rate_end = rate_end;
                line.modified = true;
            } else if (iRole == line.extrusion_role) {
                rate_end = line.volumetric_extrusion_rate_end;
            } else if (rate_end == FLT_MAX) {
                // The rate for ExtrusionRole iRole is unlimited.
                continue;
            } else {
                // Use the original, 'floating' extrusion rate as a starting point for the limiter.
            }
//            modified = false;
            float rate_start = rate_end + rate_slope * line.time_corrected();
            if (rate_start < line.volumetric_extrusion_rate_start) {
                // Limit the volumetric extrusion rate at the start of this segment due to a segment 
                // of ExtrusionType iRole, which will be extruded in the future.
                line.volumetric_extrusion_rate_start = rate_start;
                line.max_volumetric_extrusion_rate_slope_negative = rate_slope;
                line.modified = true;
//              modified = true;
            }
            feedrate_per_extrusion_role[iRole] = (iRole == line.extrusion_role) ? line.volumetric_extrusion_rate_start : rate_start;
        }
    }

    // Go forward and adjust the feedrate to decrease the slope of the extrusion rate changes.
    for (size_t i = 0; i < numExtrusionRoles; ++ i)
        feedrate_per_extrusion_role[i] = FLT_MAX;
    feedrate_per_extrusion_role[circular_buffer[idx].extrusion_role] = circular_buffer[idx].volumetric_extrusion_rate_end;

    assert(circular_buffer[idx].extruding());
    while (idx != idx_tail) {
        size_t idx_next = circular_buffer_idx_next(idx);
        for (; ! circular_buffer[idx_next].extruding() && idx_next != idx_tail; idx_next = circular_buffer_idx_next(idx_next)) ;
        if (! circular_buffer[idx_next].extruding())
        	break;
        float rate_prec = circular_buffer[idx].volumetric_extrusion_rate_end;
        // What is the gradient of the extrusion rate between idx_prev and idx?
        idx = idx_next;
        GCodeLine &line = circular_buffer[idx];
        for (size_t iRole = 1; iRole < numExtrusionRoles; ++ iRole) {
            float rate_slope = m_max_volumetric_extrusion_rate_slopes[iRole].positive;
            if (rate_slope == 0)
                // The positive rate is unlimited.
                continue;
            float rate_start = feedrate_per_extrusion_role[iRole];
            if (iRole == line.extrusion_role && rate_prec < rate_start)
                rate_start = rate_prec;
            if (line.volumetric_extrusion_rate_start > rate_start) {
                line.volumetric_extrusion_rate_start = rate_start;
                line.modified = true;
            } else if (iRole == line.extrusion_role) {
                rate_start = line.volumetric_extrusion_rate_start;
            } else if (rate_start == FLT_MAX) {
                // The rate for ExtrusionRole iRole is unlimited.
                continue;
            } else {
                // Use the original, 'floating' extrusion rate as a starting point for the limiter.
            }
            float rate_end = (rate_slope == 0) ? FLT_MAX : rate_start + rate_slope * line.time_corrected();
            if (rate_end < line.volumetric_extrusion_rate_end) {
                // Limit the volumetric extrusion rate at the start of this segment due to a segment 
                // of ExtrusionType iRole, which was extruded before.
                line.volumetric_extrusion_rate_end = rate_end;
                line.max_volumetric_extrusion_rate_slope_positive = rate_slope;
                line.modified = true;
            }
            feedrate_per_extrusion_role[iRole] = (iRole == line.extrusion_role) ? line.volumetric_extrusion_rate_end : rate_end;
        }
    }
}

void GCodePressureEqualizer::push_axis_to_output(const char axis, const float value, bool add_eol)
{
    char buf[2048];
    int len = sprintf(buf, 
        (axis == 'E') ? " %c%.3f" : " %c%.5f",
        axis, value);
    push_to_output(buf, len, add_eol);
}

void GCodePressureEqualizer::push_to_output(const char *text, const size_t len, bool add_eol)
{
    // New length of the output buffer content.
    size_t len_new = output_buffer_length + len + 1;
    if (add_eol)
        ++ len_new;

    // Resize the output buffer to a power of 2 higher than the required memory.
    if (output_buffer.size() < len_new) {
        size_t v = len_new;
        // Compute the next highest power of 2 of 32-bit v
        // http://graphics.stanford.edu/~seander/bithacks.html
        v--;
        v |= v >> 1;
        v |= v >> 2;
        v |= v >> 4;
        v |= v >> 8;
        v |= v >> 16;
        v++;
        output_buffer.resize(v);
    }

    // Copy the text to the output.
    if (len != 0) {
        memcpy(output_buffer.data() + output_buffer_length, text, len);
        output_buffer_length += len;
    }
    if (add_eol)
        output_buffer[output_buffer_length ++] = '\n';
    output_buffer[output_buffer_length] = 0;
}

void GCodePressureEqualizer::push_line_to_output(const GCodeLine &line, const float new_feedrate, const char *comment)
{
    push_to_output("G1", 2, false);
    for (size_t i = 0; i < 3; ++ i)
        if (line.pos_provided[i])
            push_axis_to_output('X'+i, line.pos_end[i]);
    push_axis_to_output('E', m_config->use_relative_e_distances.value ? (line.pos_end[3] - line.pos_start[3]) : line.pos_end[3]);
//    if (line.pos_provided[4] || fabs(line.feedrate() - new_feedrate) > 1e-5)
        push_axis_to_output('F', new_feedrate);
    // output comment and EOL
    push_to_output(comment, (comment == NULL) ? 0 : strlen(comment), true);
} 

} // namespace Slic3r