# coding: utf-8
'''
Copyright (C) 2024 brothermechanic@yandex.com

Created by brothermechanic

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
'''

import logging
import json
import math
import bpy

from .ros2bag_parser import get_animation_data

logger = logging.getLogger(__name__)


def set_animation_data(context, ros2bag_path, frame_start=0, frame_end=0, fps=30):
    ''' Set animation data from Ros2Bag database '''
    scene = context.scene

    ros2bag_data = get_animation_data(
        ros2bag_path=ros2bag_path, frame_start=frame_start, frame_end=frame_end, fps=fps)

    scene.frame_start = ros2bag_data['frame_start']
    scene.frame_end = ros2bag_data['frame_end']
    scene.render.fps = ros2bag_data['fps']
    lost_links = [item for item in ros2bag_data['scene_links'] if not bpy.data.objects.get(item)]
    if lost_links:
        logger.warning('Link(s) not found in current scene: %s', lost_links)

    for frame_data in ros2bag_data['frames_data']:
        frame_data['id']
        for link_data in frame_data['links']:
            if link_data['link'] in lost_links:
                continue

            parent_location = ((0, 0, 0) if link_data['parent'] == 'world'
                               else bpy.data.objects[link_data['parent']].location[:])

            link = bpy.data.objects[link_data['link']]
            link.rotation_mode = 'QUATERNION'

            link.location = [a + b for a, b in zip(link_data['loc_xyz'], parent_location)]
            link.rotation_quaternion = link_data['rot_wxyz']

            link.keyframe_insert(data_path='location', frame=frame_data['id'])
            link.keyframe_insert(data_path='rotation_quaternion', frame=frame_data['id'])

            link.animation_data.action.name = '{}_action'.format(link_data['link'])

    logger.info('Setup of %s links is finished!', len(ros2bag_data['scene_links']))
    return True
