#!/usr/bin/env python3

# Copyright 2023 Clearpath Robotics
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import copy
import os
import subprocess
import shlex

from turtlebot4_setup.conf import Conf, SystemOptions, BashOptions, WifiOptions, DiscoveryOptions
from turtlebot4_setup.menu import Menu, MenuEntry, OptionsMenu, Prompt, HelpMenu, PreviewMenu
from turtlebot4_setup.ros_setup import RosSetup
from turtlebot4_setup.wifi import WifiSetup


__author__ = 'Roni Kreinin'
__email__ = 'rkreinin@clearpathrobotics.com'
__copyright__ = 'Copyright © 2023 Clearpath Robotics. All rights reserved.'
__license__ = 'Apache 2.0'


class Turtlebot4Setup():
    # TurtleBot4 Setup -- https://patorjk.com/software/taag/#p=display&v=0&f=Small
    title = """
  _____         _   _     ___      _   _ _    ___      _
 |_   _|  _ _ _| |_| |___| _ ) ___| |_| | |  / __| ___| |_ _  _ _ __
   | || || | '_|  _| / -_) _ \\/ _ \\  _|_  _| \\__ \\/ -_)  _| || | '_ \\
   |_| \\_,_|_|  \\__|_\\___|___/\\___/\\__| |_|  |___/\\___|\\__|\\_,_| .__/
                                                               |_|
"""

    def __init__(self) -> None:
        self.conf = Conf()
        self.initial_conf = copy.deepcopy(self.conf)
        self.wifi = WifiSetup(self.conf)
        self.ros = RosSetup(self.conf)
        self.entries = [MenuEntry(entry='ROS Setup', function=self.ros.show),
                        MenuEntry(entry='Wi-Fi Setup', function=self.wifi.run),
                        MenuEntry(entry='Bluetooth Setup', function=self.bluetooth),
                        MenuEntry('', None),
                        MenuEntry(entry='View Settings', function=self.view_settings),
                        MenuEntry(entry='Apply Settings', function=self.apply_settings),
                        MenuEntry(entry='Reset Create3', function=self.apply_create3),
                        MenuEntry('', None),
                        MenuEntry(entry='About', function=self.about),
                        MenuEntry(entry='Help', function=self.help),
                        MenuEntry(entry='Exit', function=self.exit)]
        self.menu = Menu(self.title, self.entries)

    def bluetooth(self):
        subprocess.run(shlex.split('sudo bluetoothctl'))

    def update(self):
        o = OptionsMenu('Update TurtleBot4 Packages?.\n',
                      ['Yes', 'No'], default_option='No')

        if o.show() == 'Yes':
            subprocess.run(shlex.split('sudo apt update'))
            subprocess.run(shlex.split('sudo apt install ros-jazzy-turtlebot4-setup'))
            input()

    def view_settings(self):
        PreviewMenu([self.conf.setup_dir, self.conf.netplan_dir]).show()

    def get_settings_diff(self, options):
        diff = []

        if options is SystemOptions or \
           options is BashOptions or \
           options is WifiOptions or \
           options is DiscoveryOptions:
            for option in options:
                # None and empty string are equivalent
                if self.conf.get(option) == '' and self.initial_conf.get(option) == None or \
                   self.conf.get(option) == None and self.initial_conf.get(option) == '':
                    pass
                elif (str(self.conf.get(option)) != str(self.initial_conf.get(option))):
                    diff.append(option)

        return diff

    def settings_diff(self):
        text = ''

        diff = self.get_settings_diff(SystemOptions)
        if len(diff) > 0:
            text += '\nSystem Settings:\n'
            for option in diff:
                text += '  {0}: {1} -> {2}\n'.format(
                    option.value, self.initial_conf.get(option), self.conf.get(option))

        diff = self.get_settings_diff(BashOptions)
        if len(diff) > 0:
            text += '\nBash Settings:\n'
            for option in diff:
                text += '  {0}: {1} -> {2}\n'.format(
                    option.value, self.initial_conf.get(option), self.conf.get(option))

        diff = self.get_settings_diff(WifiOptions)
        if len(diff) > 0:
            text += '\nWi-Fi Settings:\n'
            for option in diff:
                text += '  {0}: {1} -> {2}\n'.format(
                    option.value, self.initial_conf.get(option), self.conf.get(option))

        diff = self.get_settings_diff(DiscoveryOptions)
        if len(diff) > 0:
            text += '\nDiscovery Server Settings:\n'
            for option in diff:
                text += '  {0}: {1} -> {2}\n'.format(
                    option.value, self.initial_conf.get(option), self.conf.get(option))

        if text == '':
            text = 'No changes made.\n'
        # Apply Settings -- https://patorjk.com/software/taag/#p=display&v=0&f=Small
        text = """
    _             _        ___      _   _   _
   /_\\  _ __ _ __| |_  _  / __| ___| |_| |_(_)_ _  __ _ ___
  / _ \\| '_ \\ '_ \\ | || | \\__ \\/ -_)  _|  _| | ' \\/ _` (_-<
 /_/ \\_\\ .__/ .__/_|\\_, | |___/\\___|\\__|\\__|_|_||_\\__, /__/
       |_|  |_|     |__/                          |___/    \n\n""" + text

        text += '\nApply these settings?\n'

        text += '\n**Notes**\n'
        text += '- Changes applied to ROS_DOMAIN_ID, ROBOT_NAMESPACE, RMW_IMPLEMENTATION,\n'
        text += '  or ROS_DISCOVERY_SERVER  will be applied to the Create 3 as well.\n'
        text += '- Changes applied to Wi-Fi will cause SSH sessions to hang.\n'

        return text

    def apply_settings(self):
        apply_menu = OptionsMenu(self.settings_diff, ['Yes', 'No'], default_option='No')
        if apply_menu.show() == 'Yes':
            error, msg = self.apply_ros_settings()
            if error:
                options = OptionsMenu(title='Error: Unable to set Create3 options.' +
                                      'Please ensure that the Create3 is fully booted and apply again.\n\n Details:\n' + msg,
                          menu_entries=['Okay'])
                options.show()
                return
            self.apply_wifi_settings()
            self.initial_conf = copy.deepcopy(self.conf)

    def apply_ros_settings(self):
        reinstall_job = False
        update_create3 = False

        # If one of Domain ID, Namespace, or RMW was changed, apply changes to Create 3
        for option in self.get_settings_diff(BashOptions):
            if option in [BashOptions.DOMAIN_ID, BashOptions.NAMESPACE, BashOptions.RMW]:
                update_create3 = True
            reinstall_job = True

        if len(self.get_settings_diff(DiscoveryOptions)) > 0:
            update_create3 = True
            reinstall_job = True

        for option in self.get_settings_diff(SystemOptions):
            if option is SystemOptions.MODEL:
                reinstall_job = True

        if update_create3:
            (error, result) = self.update_create3()
            if error:
                return (error, result)

        if reinstall_job:
            self.ros.robot_upstart_menu.install()
            self.ros.robot_upstart_menu.start()

        return (0, "Success")

    def apply_wifi_settings(self):
        # Run netplan apply if WiFi options have changed
        if len(self.get_settings_diff(WifiOptions)) > 0:
            subprocess.run(shlex.split('sudo netplan apply'))
            os.system('sudo reboot')

    def create3_diff(self):
        # Reset Create3 -- https://patorjk.com/software/taag/#p=display&v=0&f=Small
        text = """
  ___             _      ___              _       ____
 | _ \\___ ___ ___| |_   / __|_ _ ___ __ _| |_ ___|__ /
 |   / -_|_-</ -_)  _| | (__| '_/ -_) _` |  _/ -_)|_ \\
 |_|_\\___/__/\\___|\\__|  \\___|_| \\___\\__,_|\\__\\___|___/\n\n"""

        text += '\nReset Create3?\n'

        text += '\n**Notes**\n'
        text += '- ROS_DOMAIN_ID, ROBOT_NAMESPACE, RMW_IMPLEMENTATION will be\n'
        text += '  reapplied to the Create3.'

        return text

    def apply_create3(self):
        apply_menu = OptionsMenu(self.create3_diff, ['Yes', 'No'], default_option='No')
        if apply_menu.show() == 'Yes':
            error, msg = self.update_create3()
            if error:
                options = OptionsMenu(title='Error: Unable to set Create3 options.' +
                                      'Please ensure that the Create3 is fully booted and apply again.\n\n Details:\n' + msg,
                          menu_entries=['Okay'])
                options.show()
                return

    def update_create3(self):
        ros_domain_id = 'ros_domain_id=' + os.environ[BashOptions.DOMAIN_ID]
        ros_namespace = '&ros_namespace=' + os.environ[BashOptions.NAMESPACE]
        ros_namespace += '/_do_not_use'
        rmw_implementation = '&rmw_implementation=' + os.environ[BashOptions.RMW]

        discovery_server = f'&fast_discovery_server_value={self.conf.get_create3_server_str()}'

        create3_rmw_profile = 'config='
        if self.conf.get(DiscoveryOptions.ENABLED):
            discovery_server_enabled = '&fast_discovery_server_enabled'
            create3_rmw_profile_file = os.path.join(self.conf.setup_dir, 'fastdds_discovery_create3.xml')
            with open(create3_rmw_profile_file) as f:
                create3_rmw_profile += f.read()
        else:
            discovery_server_enabled = ''

        command = shlex.split(
            'curl -d "{0}{1}{2}{3}{4}"'.format(ros_domain_id,
                                                ros_namespace,
                                                rmw_implementation,
                                                discovery_server,
                                                discovery_server_enabled)) + \
            shlex.split('-X POST http://192.168.186.2/ros-config-save-main')

        result = subprocess.run(command, capture_output=True)

        # If the curl command fails then return and do not set any more settings.
        if (result.returncode != 0):
            return (result.returncode, "Error writing ROS settings to Create3\n\n" + result.stderr.decode("utf-8"))

        # Set create3 rmw profile
        command = shlex.split(f'curl -d {shlex.quote(create3_rmw_profile)} -X POST http://192.168.186.2/rmw-profile-override-save')

        result = subprocess.run(command, capture_output=True)

        # If the curl command fails then return and indicate the error.
        if (result.returncode != 0):
            return (result.returncode, "Error writing RMW XML Profile to Create3\n\n" + result.stderr.decode("utf-8"))

        # Set time syncing to Raspberry PI
        config = f'config=server 192.168.186.3 prefer iburst minpoll 4 maxpoll 6  # Use RPi4 server'
        command = shlex.split(f'curl -d "{config}" -X POST http://192.168.186.2/beta-ntp-conf-save')

        result = subprocess.run(command, capture_output=True)

        # If the curl command fails then return and indicate the error.
        if (result.returncode != 0):
            return (result.returncode, "Error writing NTP settings to Create3\n\n" + result.stderr.decode("utf-8"))

        # Reboot the Create3
        result = subprocess.run(shlex.split('curl -X POST http://192.168.186.2/api/reboot'), capture_output=True)

        # If the curl command fails then return and indicate the error.
        if (result.returncode != 0):
            return (result.returncode, "Error requesting Create3 to reboot\n\n" + result.stderr.decode("utf-8"))

        return (0, 'Success')

    def run(self):
        self.menu.show()
        self.initial_conf.write()

    def exit(self):
        self.menu.exit()
        self.initial_conf.write()

    def about(self):
        self.about_menu = Menu(lambda:
"""
TurtleBot 4 Open Source Robotics Platform.

Model: {0}
Version: {1}
ROS: {2}
Hostname: {3}
IP: {4}
The TurtleBot 4 was created in a partnership between Open Robotics and Clearpath Robotics.
""".format(self.conf.get(SystemOptions.MODEL),
           self.conf.get(SystemOptions.VERSION),
           self.conf.get(SystemOptions.ROS),
           self.conf.get(SystemOptions.HOSTNAME),
           self.conf.get(SystemOptions.IP)),
           [MenuEntry(entry='Model', function=self.set_model),
            MenuEntry(entry='Hostname', function=self.set_hostname),
            MenuEntry('', None),
            MenuEntry(entry='Save', function=self.save)])
        self.about_menu.show()
        self.about_menu.refresh_term_menu()

    def set_model(self):
        o = OptionsMenu('TurtleBot 4 Model\n',
                        ['standard', 'lite'],
                        default_option=self.conf.get(SystemOptions.MODEL))

        self.conf.set(SystemOptions.MODEL, o.show())

    def set_hostname(self):
        p = Prompt(prompt='Hostname [{0}]: '.format(self.conf.get(SystemOptions.HOSTNAME)),
                   default_response=self.conf.get(SystemOptions.HOSTNAME),
                   note='RPi4 Hostname')
        self.conf.set(SystemOptions.HOSTNAME, p.show())

    def save(self):
        self.conf.write()
        self.about_menu.exit()

    def help(self):
        help_menu = HelpMenu(
            'Visit the TurtleBot 4 User Manual for more details on usage. \n' +
            'https://turtlebot.github.io/turtlebot4-user-manual/')
        help_menu.show()


def main():
    setup = Turtlebot4Setup()
    setup.run()


if __name__ == '__main__':
    main()
