#!/usr/bin/env python

import unittest
import os
import math

from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoCamera, ArUcoDetector, ArUcoBoard

import cv2 as cv
import numpy

class TestDetectorParametersClass(unittest.TestCase):
    """Test DetectorParameters class."""

    def test_from_json(self):
        """Test DetectorParameters creation from json file."""
        
        # Edit file path
        current_directory = os.path.dirname(os.path.abspath(__file__))
        json_filepath = os.path.join(current_directory, 'utils/detector_parameters.json')

        # Load file
        detector_parameters = ArUcoDetector.DetectorParameters.from_json(json_filepath)

        # Check data
        self.assertEqual(detector_parameters.cornerRefinementMethod, 3)
        self.assertEqual(detector_parameters.aprilTagQuadSigma, 2)
        self.assertEqual(detector_parameters.aprilTagDeglitch, 1)

        # Check bad data access fails
        with self.assertRaises(AttributeError):

            detector_parameters.unknown_data = 1

class TestArUcoDetectorClass(unittest.TestCase):
    """Test ArUcoDetector class."""

    def test_new(self):
        """Test ArUcoDetector creation."""

        aruco_detector = ArUcoDetector.ArUcoDetector(marker_size=3)

         # Check ArUcoDetector creation
        self.assertEqual(aruco_detector.dictionary.name, 'DICT_ARUCO_ORIGINAL')
        self.assertEqual(aruco_detector.marker_size, 3)
        self.assertIsNone(numpy.testing.assert_array_equal(aruco_detector.camera.dimensions, [0, 0]))
        self.assertEqual(aruco_detector.detected_markers_number, 0)
        self.assertEqual(aruco_detector.detected_markers, {})

        aruco_dictionary = ArUcoMarkersDictionary.ArUcoMarkersDictionary('DICT_APRILTAG_16h5')
        aruco_detector = ArUcoDetector.ArUcoDetector(aruco_dictionary, 5.2)

         # Check ArUcoDetector creation
        self.assertEqual(aruco_detector.dictionary.name, 'DICT_APRILTAG_16h5')
        self.assertEqual(aruco_detector.marker_size, 5.2)
        self.assertIsNone(numpy.testing.assert_array_equal(aruco_detector.camera.dimensions, [0, 0]))
        self.assertEqual(aruco_detector.detected_markers_number, 0)
        self.assertEqual(aruco_detector.detected_markers, {})

    def test_from_json(self):
        """Test ArUcoDetector creation."""

        # Edit file path
        current_directory = os.path.dirname(os.path.abspath(__file__))
        json_filepath = os.path.join(current_directory, 'utils/detector.json')

        # Load file
        aruco_detector = ArUcoDetector.ArUcoDetector.from_json(json_filepath)

         # Check ArUcoDetector creation
        self.assertEqual(aruco_detector.dictionary.name, 'DICT_ARUCO_ORIGINAL')
        self.assertEqual(aruco_detector.marker_size, 3)
        self.assertIsNone(numpy.testing.assert_array_equal(aruco_detector.camera.dimensions, [1920, 1080]))
        self.assertEqual(aruco_detector.parameters.cornerRefinementMethod, 3)
        self.assertEqual(aruco_detector.parameters.aprilTagQuadSigma, 2)
        self.assertEqual(aruco_detector.parameters.aprilTagDeglitch, 1)

    def test_detect(self):
        """Test detect method."""

        aruco_detector = ArUcoDetector.ArUcoDetector(marker_size=3)

        # Load picture Full HD to test ArUcoMarker detection
        current_directory = os.path.dirname(os.path.abspath(__file__))
        frame = cv.imread(os.path.join(current_directory, 'utils/full_hd_marker.png'))

        # Check ArUcoMarker detection
        aruco_detector.detect_markers(frame)

        self.assertEqual(aruco_detector.detected_markers_number, 1)

        self.assertEqual(aruco_detector.detected_markers[0].dictionary, aruco_detector.dictionary)
        self.assertEqual(aruco_detector.detected_markers[0].identifier, 0)
        self.assertEqual(aruco_detector.detected_markers[0].size, 3)

        # Check corner positions with -/+ 10 pixels precision
        self.assertIsNone(numpy.testing.assert_almost_equal(aruco_detector.detected_markers[0].corners[0][0].astype(int), numpy.array([3823, 2073]), decimal=-1))
        self.assertIsNone(numpy.testing.assert_almost_equal(aruco_detector.detected_markers[0].corners[0][1].astype(int), numpy.array([4177, 2073]), decimal=-1))
        self.assertIsNone(numpy.testing.assert_almost_equal(aruco_detector.detected_markers[0].corners[0][2].astype(int), numpy.array([4177, 2427]), decimal=-1))
        self.assertIsNone(numpy.testing.assert_almost_equal(aruco_detector.detected_markers[0].corners[0][3].astype(int), numpy.array([3823, 2427]), decimal=-1))

        # Check marker pose estimation
        aruco_detector.estimate_markers_pose([0])

        # Check marker translation with -/+ 0.1 cm precision and rotation with -/+ 0.001 radian precision
        self.assertIsNone(numpy.testing.assert_almost_equal(aruco_detector.detected_markers[0].translation, numpy.array([33.87, 19.05, 0.]), decimal=1))
        self.assertIsNone(numpy.testing.assert_almost_equal(aruco_detector.detected_markers[0].rotation, numpy.array([[1., 0., 0.], [0., -1., 0.], [0., 0., -1.]]), decimal=3))

        # Check detect metrics
        detect_count, markers_count = aruco_detector.detection_metrics
        self.assertEqual(detect_count, 1)
        self.assertEqual(markers_count[0], 1)

    def test_detect_board(self):
        """Test detect board method."""

        aruco_board = ArUcoBoard.ArUcoBoard(7, 5, 5, 3)
        aruco_detector = ArUcoDetector.ArUcoDetector(marker_size=3)

        # Load picture Full HD to test ArUcoMarker board detection
        current_directory = os.path.dirname(os.path.abspath(__file__))
        frame = cv.imread(os.path.join(current_directory, 'utils/full_hd_board.png'))

        # Check ArUcoMarker board detection
        aruco_detector.detect_board(frame, aruco_board, aruco_board.markers_number)

        self.assertEqual(aruco_detector.board_corners_number, aruco_board.corners_number)
        self.assertEqual(len(aruco_detector.board_corners), 24)
        self.assertEqual(len(aruco_detector.board_corners_identifier), 24)

if __name__ == '__main__':

    unittest.main()