# 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 config
import logging
import time
import threading
import random

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"
    M0 = "M0"
    M1 = "M1"
    REPORT_SD_CARD_PRINT_STATUS_GCODE = "M27"
    AMBIENT_TEMPERATURE = 21.0
    TEMPERATURE_FLUCTUATION = 0.4

    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.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 = collections.deque(['start'])
        #self.out_buffer.append('start')
        self.temps = [self.AMBIENT_TEMPERATURE, self.AMBIENT_TEMPERATURE]
        self.target_temps = [0.0, 0.0]
        self.position = [0.0, 0.0, 0.0, 0.0]
        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.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
        else:
            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):
        return f"ok T:{self.get_fluctuated_temp(self.temps[1])}/{self.target_temps[1]} B:{self.get_fluctuated_temp(self.temps[0])}/{self.target_temps[0]}"

    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}')
                        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] == 'M115':
                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)
                if ttemperature != None:
                    self.target_temps[1] = 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)
                if ttemperature != None:
                    self.target_temps[1] = ttemperature
                    self.wait_heatup[1] = 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.logger.info(f'Sdcard upload mode. File: {self.selected_filesname}')
                return
            elif args[0] == self.FINISH_UPLOADING_GCODE:
                self.logger.info('Sdcard upload mode off')
                self.sdcard_upload_mode = False
                self.out_buffer.append("ok") # yeah two ok should be send as a resp. One for M29 and one for M28. That is how it works on RepRapFirmware
            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")
            if not no_resp:
                self.out_buffer.append("ok")

    def recv(self, size=None):
        if self.out_buffer:
            if size:
                data = self.out_buffer[0][:1:]
                self.out_buffer[0] = self.out_buffer[0][1:]
            else:
                data = self.out_buffer.popleft()
            if self.verbose and data:
                self.logger.info('RECV: ' + str(data))
            return data.encode()
        elif self.out_buffer == None:
            self.logger.warning('Receive called on closed port')
            time.sleep(1)
        time.sleep(self.RECV_PAUSE_ON_NONE)
        return b""

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

    def send(self, data, raw=False, allow_verbose=True):
        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 allow_verbose:
                        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 close(self):
        self.logger.info('Port closed')
        self.in_buffer = b""
        self.out_buffer = []
        self.port = ""
