#!/usr/bin/env python3

# stdlib
import os
import argparse
from datetime import datetime

import jinja2

# 3rd-party
import rospkg
from atom_core.utilities import atomError, atomStartupPrint
from jinja2 import Environment, FileSystemLoader
from colorama import Style, Fore

# local packages
from atom_core.system import execute, resolvePath

if __name__ == "__main__":

    # Parse command line arguments
    ap = argparse.ArgumentParser()
    ap.add_argument("-n", "--name", help='package_name', type=str, required=True)
    args = vars(ap.parse_args())

    # Initial setup
    dt_string = datetime.now().strftime("%d/%m/%Y %H:%M:%S")
    package_path = resolvePath(args['name'])  # full path to the package, including its name.
    package_name = os.path.basename(package_path)

    atomStartupPrint('Creating calibration package ' + Fore.BLUE + package_name + Style.RESET_ALL)

    package_base_path = os.path.dirname(package_path)  # parent path where the package is located
    rospack = rospkg.RosPack()
    atom_calibration_path = rospack.get_path('atom_calibration')

    # Check if folder exists, abort if so
    if os.path.exists(package_path):
        atomError('Folder ' + Fore.BLUE + package_path + Style.RESET_ALL +
                  ' already exists. Remove or try a different name for your package.')

    # Check if a package with the same name exists under ROS, abort if so
    if package_name in rospack.list():
        atomError('A ROS package with the same name (' + Fore.BLUE + package_name + Style.RESET_ALL +
                  ') already exists under your $ROS_PACKAGE_PATH. Try a different name for your package.')

    # Create package
    print('Creating package ' + Fore.BLUE + package_name + Style.RESET_ALL +
          ' on folder ' + Fore.BLUE + package_base_path + Style.RESET_ALL)
    cmd = 'cd ' + package_base_path + ' && catkin_create_pkg ' + package_name + ' rospy atom_calibration ' + \
          '-a "Auto generated by Atomic Framework" ' + \
          '-D "Calibration package created automatically by the atomic_calibration/create_calibration_pkg ' \
          'script. Check https://lardemua.github.io/atom_documentation/ for more information."'
    execute(cmd, verbose=True)

    # Augment package structure (e.g. create folders, etc)
    execute('mkdir ' + package_path + '/calibration')
    execute('mkdir ' + package_path + '/launch')
    execute('mkdir ' + package_path + '/rviz')
    execute('mkdir ' + package_path + '/urdf')
    execute('mkdir ' + package_path + '/urdf/optimized')
    execute('mkdir ' + package_path + '/scripts')

    # Template jinja engine setup
    file_loader = FileSystemLoader(atom_calibration_path + '/templates')
    env = Environment(loader=file_loader, undefined=jinja2.StrictUndefined)
    env.add_extension('jinja2.ext.do')

    # Add configure.py script
    configure_file = package_path + '/scripts/configure'

    template = env.get_template('configure')
    with open(configure_file, 'wb') as f:
        output = template.render(c={'date': dt_string,
                                    'package_name': package_name})
        f.write(output.encode('utf-8'))

    execute('chmod +x ' + configure_file)  # make file executable

    # Add template config.yml
    config_yaml_file = package_path + '/calibration/config.yml'
    template = env.get_template('config.yml')
    with open(config_yaml_file, 'wb') as f:
        output = template.render(c={'date': dt_string,
                                    'package_name': package_name})
        f.write(output.encode('utf-8'))

    # Check if package is under $ROS_PACKAGE_PATH
    execute('rospack profile && rospack profile && rospack profile', verbose=False)
    if package_name in rospack.list():
        print(Fore.YELLOW + 'Package ' + package_name +
              'not found under ROS. Are you sure the path you gave in under your $ROS_PACKAGE_PATH? Calibration '
              'package will not work if it is not under the $ROS_PACKAGE_PATH. Please fix this before running the '
              'package configuration. ' + Style.RESET_ALL)

    # Print final report

    print('\nCreated calibration package ' + Fore.BLUE + package_name + Style.RESET_ALL +
          ' in ' + Fore.BLUE + package_base_path + Style.RESET_ALL + '. Next steps:')
    print(Fore.BLUE + '   1. Edit the  config.yml file to setup the calibration and; \n   2. Run:' + Fore.RED +
          ' rosrun ' + package_name + ' configure' + Style.RESET_ALL)