# Copyright 3D Control Systems, Inc. All Rights Reserved 2017-2019.
# Built in San Francisco.

# This software is distributed under a commercial license for personal,
# educational, corporate or any other use.
# The software as a whole or any parts of it is prohibited for distribution or
# use without obtaining a license from 3D Control Systems, Inc.

# All software licenses are subject to the 3DPrinterOS terms of use
# (available at https://www.3dprinteros.com/terms-and-conditions/),
# and privacy policy (available at https://www.3dprinteros.com/privacy-policy/)

import collections
import logging
import time
import threading
import random

import config
import log


class EmulatedSerialConnection:

    DEFAULT_GCODE_EXEC_SPEED = {"G0" : 0.00001, "G1" : 0.00001, "G28" : 2}
    #DEFAULT_GCODE_EXEC_SPEED = {"G0" : 0.001, "G1" : 0.001, "G28" : 2}
    IDLE_SLEEP = 0.1
    HEATING_SPEEDS = [10.0, 30.0]
    HEATING_WAITING_ACK_PERIOD = 1
    HEATING_MISS_THRD = 10 #warning, setting it lower can cause overstep of target temp
    DEFAULT_ERROR_RATE = 0.0
    RESEND_RANGE = 20
    RECV_PAUSE_ON_NONE = 0.01
    RECV_PAUSE_ON_ACK = 0.001
    START_UPLOADING_GCODE = "M28"
    FINISH_UPLOADING_GCODE = "M29"
    FINISH_UPLOADING_GCODE_BYTES = b"M29"
    SELECT_FILE = "M23"
    START_PRINTING_FILE = "M24"
    PAUSE_PRINTING_FILE = "M25"
    CALL_MACRO = "M98"
    M0 = "M0"
    M1 = "M1"
    T = "T"
    REPORT_SD_CARD_PRINT_STATUS_GCODE = "M27"
    AMBIENT_TEMPERATURE = 21.0
    TEMPERATURE_FLUCTUATION = 0.4
    DEFAULT_NUMBER_OF_TOOLS = 2

    def __init__(self, port_name="/dev/null", baudrate=115200, timeout=1, start_dtr=None, logger=None):
        if logger:
            self.logger = logger.getChild('connection')
        else:
            self.logger = logging.getLogger(self.__class__.__name__)
        self.port_name = port_name
        self.baudrate = baudrate
        self.read_timeout = timeout
        self.write_timeout = timeout
        self.sdcard_files = {}
        self.selected_filesname = ""
        self.sdcard_upload_mode = False
        self.sdcard_printing_mode = False
        self.sdcard_pause = False
        self.sdcard_total_size = 0
        self.sdcard_current_byte = 0
        self.verbose = config.get_settings()['verbose']
        self.in_buffer = b""
        self.out_buffer = collections.deque()
        self.out_buffer.append('start')
        self.wait_heatup = [False, False]
        self.port = port_name
        self.resend_error_rate = self.DEFAULT_ERROR_RATE
        self.error_on_line_number = -1
        self.current_line_number = 0
        self.current_tool_index = 0
        self.number_of_tools = int(config.get_settings()['virtual_printer'].get('tools_number', self.DEFAULT_NUMBER_OF_TOOLS))
        self.temps = [self.AMBIENT_TEMPERATURE]
        self.target_temps = [0.0]
        self.position = [0.0, 0.0, 0.0]
        for _ in range(0, self.number_of_tools):
            self.temps.append(self.AMBIENT_TEMPERATURE)
            self.target_temps.append(self.AMBIENT_TEMPERATURE)
            self.position.append(0.0)
            self.wait_heatup.append(False)
        self.exec_thread = threading.Thread(target=self.gcode_execution_loop)
        self.exec_thread.start()
        self.in_buffer_lock = threading.RLock()

    def get_fluctuated_temp(self, temp):
        return round(temp + self.TEMPERATURE_FLUCTUATION - \
                (2*random.random()*self.TEMPERATURE_FLUCTUATION), 1)

    def set_print_speed_coef(self, coef):
        for key in self.DEFAULT_GCODE_EXEC_SPEED:
            self.DEFAULT_GCODE_EXEC_SPEED[key] = self.DEFAULT_GCODE_EXEC_SPEED[key] * coef

    def parse_arg(self, arg, arg_name, arg_type):
        try:
            parsed = arg_type(arg.replace(arg_name, ''))
        except ValueError:
            error = f'Error: invalid gcode arg:{arg}'
            self.out_buffer.append(error)
            self.logger.warning(error)
            self.logger.debug(f'Arg:{arg} Arg_name:{arg_name}, Arg_type:{arg_type}')
            parsed = None
        return parsed

    def search_and_parse_arg(self, args, arg_name, arg_type):
        for arg in args:
            if arg_name in arg:
                return self.parse_arg(arg, arg_name, arg_type)

    def validate_checksum(self, line, given_checksum):
        checksum = 0
        for char in line.encode():
            checksum ^= char
        #self.logger.debug(f"Line:{line} Checksum:{given_checksum} Expected:{checksum}")
        if checksum == int(given_checksum):
            return True
        error = f'Error:checksum mismatch, Last Line: {self.current_line_number}'
        self.out_buffer.append(error)
        self.logger.warning(error)
        return False

    def form_tempeatures_resp(self):
        line = f"ok T:{self.get_fluctuated_temp(self.temps[self.current_tool_index + 1])}/{self.target_temps[self.current_tool_index + 1]}"
        for index in range(0, self.number_of_tools):
            line += f" T{index}:{self.get_fluctuated_temp(self.temps[index + 1])}/{self.target_temps[index + 1]}"
        line += f" B:{self.get_fluctuated_temp(self.temps[0])}/{self.target_temps[0]}"
        return line

    def is_waiting_heatup(self):
        for wait in self.wait_heatup:
            if wait:
                self.out_buffer.append(self.form_tempeatures_resp().strip('ok '))
                return True
        return False

    @log.log_exception
    def gcode_execution_loop(self):
        while self.port:
            last_loop_time = time.monotonic()
            #self.logger.debug("HW", self.wait_heatup, 'TT', self.target_temps, 'T', self.temps)
            if self.is_waiting_heatup():
                time.sleep(self.HEATING_WAITING_ACK_PERIOD)
            elif not self.in_buffer and (not self.sdcard_printing_mode or self.sdcard_pause):
                time.sleep(self.IDLE_SLEEP)
            else:
                no_resp = False
                with self.in_buffer_lock:
                    endofline_index = self.in_buffer.find(b"\n")
                    if endofline_index != -1:
                        gcode = self.in_buffer[:endofline_index]
                        self.in_buffer = self.in_buffer[endofline_index+1:]
                    else:
                        gcode = None
                if not gcode:
                    if self.sdcard_printing_mode:
                        with self.in_buffer_lock:
                            endofline_index = self.sdcard_files[self.selected_filesname].find(b"\n")
                            if endofline_index != -1:
                                gcode = self.sdcard_files[self.selected_filesname][:endofline_index]
                                self.sdcard_current_byte += len(gcode)
                                self.sdcard_files[self.selected_filesname] = self.sdcard_files[self.selected_filesname][endofline_index+1:]
                                no_resp = True
                                if self.verbose:
                                    self.logger.info(F'SDCARD SEND: {gcode}')
                if gcode:
                    if self.RESEND_RANGE and random.random() < self.resend_error_rate or self.current_line_number == self.error_on_line_number:
                        if self.current_line_number == self.error_on_line_number:
                            self.error_on_line_number = -1
                        resent_from = random.randrange(self.current_line_number-self.RESEND_RANGE, self.current_line_number)
                        if resent_from < 0:
                            resent_from = 0
                        self.current_line_number = resent_from
                        self.logger.info('Sending resend from: ' + str(resent_from))
                        self.out_buffer.append('Resend: ' + str(resent_from))
                        self.out_buffer.append('ok')
                    else:
                        self.process_gcode(gcode, no_resp)
            loop_delta = time.monotonic() - last_loop_time
            for index, ttemp in enumerate(self.target_temps):
                if not ttemp:
                    ttemp = self.AMBIENT_TEMPERATURE
                if int(ttemp) > int(self.temps[index]):
                    self.temps[index] = round(min(self.temps[index] + self.HEATING_SPEEDS[index] * loop_delta, ttemp), 1)
                elif int(ttemp) < int(self.temps[index]):
                    self.temps[index] = round(max(self.temps[index] - self.HEATING_SPEEDS[index] * loop_delta, ttemp), 1)
                else:
                    if self.wait_heatup[index]:
                        if index == 0:
                            self.logger.info(f'Heating complete for bed')
                        else:
                            self.logger.info(f'Heating complete for T{index-1}')
                        self.wait_heatup[index] = False
                        self.out_buffer.append("ok")

    def process_gcode(self, gcode, no_resp):
        try:
            gcode = gcode.decode(errors='ignore')
        except:
            self.out_buffer.append(f'Error: unable to decode a gcode line: {gcode}')
        gcode_and_check_sum_list = gcode.split('*')
        gcode = gcode_and_check_sum_list[0]
        if len(gcode_and_check_sum_list) > 1:
            checksum = gcode_and_check_sum_list[1]
        else:
            checksum = None
        args = gcode.split()
        if args:
            if 'M110' in args:
                for arg in args:
                    if 'N' in arg:
                        line_number = self.parse_arg(arg, 'N', int)
                        if line_number != None:
                            self.current_line_number = line_number + 1
                            self.out_buffer.append("ok")
                            return
                else:
                    message = f'Error. Unknown command: {gcode}'
                    self.logger.warning(message)
                    self.out_buffer.append(message)
                self.out_buffer.append("ok")
                return
            elif args[0].startswith('N'):
                if checksum == None:
                    self.logger.warning(f'Warning:checksum mismatch: {gcode} {checksum}')
                    self.out_buffer.append(f'Error:checksum mismatch, Last Line: {self.current_line_number}')
                    return
                elif not self.validate_checksum(gcode, checksum):
                    self.logger.warning(f'Warning:checksum mismatch: {gcode} {checksum}')
                    self.out_buffer.append(f'Error:checksum mismatch, Last Line: {self.current_line_number}')
                    return
                line_number = self.parse_arg(args[0], 'N', int)
                if line_number == None or line_number != self.current_line_number:
                    self.out_buffer.append(f'Line number is not last line number+1. Last line: {self.current_line_number}') #TODO
                    self.out_buffer.append('Resend: ' + str(self.current_line_number))
                    self.out_buffer.append('ok')
                    return
                else:
                    self.current_line_number += 1
                    args = args[1:]
            for key in self.DEFAULT_GCODE_EXEC_SPEED:
                if key and args[0].startswith(key):
                    time.sleep(self.DEFAULT_GCODE_EXEC_SPEED[key])
                    break
            if args[0] == 'M105':
                self.out_buffer.append(self.form_tempeatures_resp())
                return
            elif args[0] == 'M114':
                self.out_buffer.append(f"ok X:{self.position[0]} Y:{self.position[1]} Z:{self.position[2]} E:{self.position[3]}")
                return
            elif args[0] == 'M104':
                ttemperature = self.search_and_parse_arg(args, 'S', float)
                tool_index = self.search_and_parse_arg(args, 'T', int)
                if tool_index is None:
                    tool_index = self.current_tool_index + 1
                if ttemperature != None:
                    self.target_temps[tool_index] = ttemperature
                    self.logger.info(f"Setting T0 target temp: {ttemperature}")
            elif args[0] == 'M140':
                ttemperature = self.search_and_parse_arg(args, 'S', float)
                if ttemperature != None:
                    self.target_temps[0] = ttemperature
                    self.logger.info(f"Setting bed target temp: {ttemperature}")
            elif args[0] == 'M109':
                ttemperature = self.search_and_parse_arg(args, 'S', float)
                tool_index = self.search_and_parse_arg(args, 'T', int)
                if tool_index is None:
                    tool_index = self.current_tool_index + 1
                if ttemperature != None:
                    self.target_temps[tool_index] = ttemperature
                    self.wait_heatup[tool_index] = True
                    self.logger.info(f"Setting T0 target temp: {ttemperature}")
                    return
            elif args[0] == 'M190':
                ttemperature = self.search_and_parse_arg(args, 'S', float)
                if ttemperature != None:
                    self.target_temps[0] = ttemperature
                    self.wait_heatup[0] = True
                    self.logger.info(f"Setting bed target temp: {ttemperature}")
                    return
            elif args[0] == self.START_UPLOADING_GCODE:
                self.sdcard_printing_mode = False
                self.selected_filesname = args[1].strip()
                self.sdcard_files[self.selected_filesname] = b""
                self.sdcard_upload_mode = True
                self.out_buffer.append("ok") 
                self.logger.info(f'Sdcard upload mode. File: {self.selected_filesname}')
            elif args[0] == self.FINISH_UPLOADING_GCODE:
                self.logger.info('Sdcard upload mode off')
                self.sdcard_upload_mode = False
            elif args[0] == self.SELECT_FILE:
                self.selected_filesname = args[1].strip()
                self.sdcard_total_size = len(self.sdcard_files.get(self.selected_filesname, 0))
                self.sdcard_current_byte = 0
            elif args[0] == self.START_PRINTING_FILE:
                self.logger.info('Sdcard print mode')
                self.sdcard_printing_mode = True
                self.sdcard_pause = False
            elif args[0] == self.PAUSE_PRINTING_FILE:
                if self.sdcard_printing_mode:
                    self.logger.info('Sdcard pause')
                    self.sdcard_pause = True
                else:
                    self.out_buffer.append("Error. Can't execute M25 - not printing from sdcard")
            elif args[0] == self.M0:
                if self.sdcard_printing_mode:
                    self.sdcard_current_byte = 0
                    self.logger.info('Sdcard print mode abort')
                    self.sdcard_printing_mode = False
            elif args[0] == self.M0 or args[0] == self.M1:
                if self.sdcard_printing_mode:
                    self.sdcard_current_byte = 0
                    self.logger.info('Sdcard print mode abort')
                    self.sdcard_printing_mode = False
            elif args[0] == self.REPORT_SD_CARD_PRINT_STATUS_GCODE:
                if self.sdcard_printing_mode:
                    self.out_buffer.append(f"SD printing byte {self.sdcard_current_byte}/{self.sdcard_total_size}")
                else:
                    self.out_buffer.append(f"SD not printing")
            elif args[0] == self.CALL_MACRO:
                if len(args) > 1:
                    if 'prep_z_calib.g' in args[1]:
                        self.out_buffer.append(f"ok X:250.0 Y:100.0 Z:0.0 E:0.0")
                        self.no_resp = True
            elif args[0].startswith(self.T):
                tool_index = self.get_tool_index(args[0])
                if tool_index is not None:
                    self.current_tool_index = tool_index
            if not no_resp:
                self.out_buffer.append("ok")

    def get_tool_index(self, arg_word):
        try:
            tool_index = int(arg_word.strip(self.T))
        except:
            self.logger.warning('Invalid tool change arg: ' + str(arg_word))
        else:
            if tool_index < self.number_of_tools:
                return tool_index

    def recv(self, size=None):
        if self.out_buffer == None:
            self.logger.warning('Receive called on closed port')
            time.sleep(self.read_timeout/10)
            return None
        timeleft = self.read_timeout
        step = self.read_timeout/10000
        while not self.out_buffer:
            if timeleft > 0:
                time.sleep(step)
                timeleft -= step
            else:
                if self.verbose:
                    self.logger.info('RECV: ""')
                return b""
        data = self.out_buffer.popleft()
        if self.verbose:
            self.logger.info('RECV: ' + str(data))
        return data.encode()

    def prepare_data(self, data):
        return data.strip() + b"\n"

    def send(self, data, raw=False):
        if not raw:
            data = self.prepare_data(data)
        with self.in_buffer_lock:
            if self.sdcard_upload_mode:
                if data.startswith(self.FINISH_UPLOADING_GCODE_BYTES) and \
                    data.split()[0] == self.FINISH_UPLOADING_GCODE_BYTES:
                    self.sdcard_upload_mode = False
                    self.logger.info(f'SD card upload complete. Size: {len(self.in_buffer)}')
                    self.in_buffer += data
                else:
                    self.sdcard_files[self.selected_filesname] += data
                    if self.verbose and not raw:
                        self.logger.info(f'SD card buffer len: {len(self.sdcard_files[self.selected_filesname])}')
                return True
            elif self.in_buffer is not None:
                if self.in_buffer:
                    self.logger.warning(f"Received a gcode when in_buffer is not empty. Buffer:{self.in_buffer}. Recv:{data}\n")
                self.in_buffer += data
                if self.verbose:
                    self.logger.info('SEND: ' + str(data))
                return True
            self.logger.warning('Send called on closed port')
            return False

    def reset(self):
        self.logger.info('Port reset')

    def set_error_rate(self, rate):
        self.resend_error_rate = rate

    def set_error_on_line_number(self, line_number):
        self.error_on_line_number = line_number

    def flush_recv(self):
        self.out_buffer.clear()

    def close(self):
        self.logger.info('Port closed')
        self.in_buffer = b""
        self.out_buffer.clear()
        self.port = ""
