#!/usr/bin/env python
# vim:set ts=4 sw=4 et:
#
# Copyright 2015 Vladimir Ermakov.
#
# This file is part of the mavros package and subject to the license terms
# in the top-level LICENSE file of the mavros repository.
# https://github.com/mavlink/mavros/tree/master/LICENSE.md

from __future__ import print_function

import os.path
import sys
import argparse

import rospy
import mavros

from mavros import ftp
from stat import S_IFDIR, S_IFREG

try:
    from fuse import FUSE, FuseOSError, Operations, \
            fuse_get_context, ENOENT
except ImportError:
    print("Fuse wrapper module not found. Please install fusepy: pip install fusepy", file=os.stderr)
    os.exit(1)


class RosLoggingMixIn(object):
    """
    Same as fuse.LoggingMixIn but uses rospy logging
    """

    def __call__(self, op, path, *args):
        rospy.logdebug('-> %s %s %s', op, path, repr(args))
        ret = '[Unhandled Exception]'
        try:
            ret = getattr(self, op)(path, *args)
            return ret
        except OSError, e:
            ret = str(e)
            raise
        finally:
            rospy.logdebug('<- %s %s', op, repr(ret))


def ioerror_to_oserror(foo):
    def decorator(*args, **kvargs):
        try:
            return foo(*args, **kvargs)
        except IOError as e:
            raise OSError(e.errno, e.message)

    return decorator


class MavFtp(RosLoggingMixIn, Operations):
    """
    MAVLink-FTP wrapper for mavros ftp plugin.

    Based on fusepy SFTP example.
    """

    def __init__(self):
        self._attr_cache = {}

    @staticmethod
    def _make_attr(dir_path, file_entry):
        """
        Make stat attr dict from FileEntry object.

        Item mode are set for PX4:
            - / : ROMFS (read only)
            - /fs/microsd : MicroSD FAT (read/write)
        """

        uid, gid, pid = fuse_get_context()

        dir_mode, file_mode = S_IFDIR | 0555, S_IFREG | 0444
        if dir_path.startswith('/fs/microsd'):
            dir_mode, file_mode = S_IFDIR | 0755, S_IFREG | 0644

        return {
            'st_mode': file_mode if file_entry.type == ftp.FileEntry.TYPE_FILE else dir_mode,
            'st_size': file_entry.size,
            'st_uid': uid,
            'st_gid': gid,
        }

    def _update_attr_cache(self, dir_path, fe_list):
        rospy.logdebug("update attr cache for: %s", ', '.join((os.path.join(dir_path, fe.name) for fe in fe_list)))
        self.fsyncdir(dir_path)
        for fe in fe_list:
            key = os.path.join(dir_path, fe.name)
            self._attr_cache[key] = MavFtp._make_attr(dir_path, fe)

    def _update_attr_size(self, path, size):
        if self._attr_cache.has_key(path):
            self._attr_cache[path]['st_size'] = size

    def destroy(self, path):
        pass

    def getattr(self, path, fh=None):
        if path == '/':
            uid, gid, pid = fuse_get_context()
            return {
                'st_mode': S_IFDIR | 0755,
                'st_uid': uid,
                'st_gid': gid,
            }

        if self._attr_cache.has_key(path):
            return self._attr_cache[path]

        try:
            dir_path = os.path.dirname(path)
            fe_list = ftp.listdir(dir_path)
            self._update_attr_cache(dir_path, fe_list)
            return self._attr_cache[path]
        except IOError as e:
            raise OSError(e.errno, e.message)
        except KeyError:
            raise FuseOSError(ENOENT)

    @ioerror_to_oserror
    def readdir(self, path, fh):
        fe_list = ftp.listdir(path)
        self._update_attr_cache(path, fe_list)
        return ['.', '..'] + [fe.name for fe in fe_list if fe not in ('.', '..')]

    def fsyncdir(self, path, datasync=None, fh=None):
        for k in self._attr_cache.keys():
            if k.startswith(path):
                del self._attr_cache[k]

    @ioerror_to_oserror
    def mkdir(self, path, mode):
        ftp.mkdir(path)

    @ioerror_to_oserror
    def rmdir(self, path):
        self.fsyncdir(path)
        ftp.rmdir(path)

    @ioerror_to_oserror
    def create(self, path, mode):
        with ftp.open(path, 'cw'):
            return 0

    @ioerror_to_oserror
    def read(self, path, size, offset, fh):
        with ftp.open(path, 'r') as fd:
            self._update_attr_size(path, fd.size)
            fd.seek(offset)
            return str(fd.read(size))

    @ioerror_to_oserror
    def write(self, path, data, offset, fh):
        with ftp.open(path, 'w') as fd:
            fd.seek(offset)
            fd.write(data)
            self._update_attr_size(path, fd.size)
            return len(data)

    @ioerror_to_oserror
    def unlink(self, path):
        self.fsyncdir(path)
        ftp.unlink(path)

    @ioerror_to_oserror
    def rename(self, old, new):
        self.fsyncdir(old)
        ftp.rename(old, new)

    @ioerror_to_oserror
    def truncate(self, path, length, fh=None):
        with ftp.open(path, 'w') as fd:
            fd.truncate(length)


def main():
    parser = argparse.ArgumentParser(description="FUSE for MAVLink-FTP mavros plugin")
    parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default=mavros.DEFAULT_NAMESPACE)
    parser.add_argument('-v', '--verbose', action='store_true', help="verbose output")
    parser.add_argument('-d', '--debug', action='store_true', help="libfuse debug")
    parser.add_argument('path', type=str, help="mount point")

    args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:])

    rospy.init_node("mavftp_fuse", log_level=rospy.DEBUG if args.verbose else None)
    mavros.set_namespace(args.mavros_ns)

    fuse = FUSE(MavFtp(), args.path, foreground=True, debug=args.debug)


if __name__ == '__main__':
    main()

