# Copyright (C) 2021 Open Source Robotics Foundation
#
# 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 unittest
import math
from ignition.math import Vector2d
from ignition.math import Vector2f


class TestVector2(unittest.TestCase):

    def test_construction(self):
        v = Vector2d()
        self.assertAlmostEqual(0.0, v.x())
        self.assertAlmostEqual(0.0, v.y())

        vec = Vector2d(1, 0)
        self.assertEqual(vec.x(), 1)
        self.assertEqual(vec.y(), 0)

        vec2 = Vector2d(vec)
        self.assertEqual(vec2, vec)

        # Copy
        vec3 = vec
        self.assertEqual(vec3, vec)

        # Inequality
        vec4 = Vector2d()
        self.assertNotEqual(vec, vec4)

    def test_vector2(self):
        v = Vector2d(1, 2)

        # Distance
        self.assertAlmostEqual(2.236, v.distance(Vector2d()), delta=1e-2)

        # Normalize
        v.normalize()
        self.assertTrue(v.equal(Vector2d(0.447214, 0.894427), 1e-4))

        # Set
        v.set(4, 5)
        self.assertTrue(v.equal(Vector2d(4, 5), 1e-4))

        # Abs
        v.set(-1, -2)
        self.assertTrue(v.abs().equal(Vector2d(1, 2), 1e-4))

        # _eq_
        v = Vector2d(6, 7)
        self.assertTrue(v.equal(Vector2d(6, 7), 1e-4))

        # _add_
        v = v + Vector2d(1, 2)
        self.assertTrue(v.equal(Vector2d(7, 9), 1e-4))

        v += Vector2d(5, 6)
        self.assertTrue(v.equal(Vector2d(12, 15), 1e-4))

        # __sub__
        v = v - Vector2d(2, 4)
        self.assertTrue(v.equal(Vector2d(10, 11), 1e-4))

        v.set(2, 4)
        v -= Vector2d(1, 6)
        self.assertTrue(v.equal(Vector2d(1, -2), 1e-4))

        # __truediv__
        v.set(10, 6)
        v = v / Vector2d(2, 3)
        self.assertTrue(v.equal(Vector2d(5, 2), 1e-4))

        v.set(10, 6)
        v /= Vector2d(2, 3)
        self.assertTrue(v.equal(Vector2d(5, 2), 1e-4))

        # __truediv__ int
        v.set(10, 6)
        v = v / 2
        self.assertTrue(v.equal(Vector2d(5, 3), 1e-4))

        v.set(10, 6)
        v /= 2
        self.assertTrue(v.equal(Vector2d(5, 3), 1e-4))

        # __mul__
        v.set(10, 6)
        v = v * Vector2d(2, 4)
        self.assertTrue(v.equal(Vector2d(20, 24), 1e-4))

        v.set(10, 6)
        v *= Vector2d(2, 4)
        self.assertTrue(v.equal(Vector2d(20, 24), 1e-4))

        # __mul__ int
        v.set(10, 6)
        v = v * 2
        self.assertTrue(v.equal(Vector2d(20, 12), 1e-4))

        v.set(10, 6)
        v *= 2
        self.assertTrue(v.equal(Vector2d(20, 12), 1e-4))

        # is_finite
        self.assertTrue(v.is_finite())

    def test_max(self):
        vec1 = Vector2d(0.1, 0.2)
        vec2 = Vector2d(0.3, 0.5)
        vec3 = Vector2d(0.4, 0.2)

        self.assertAlmostEqual(vec1.max(), 0.2)
        self.assertAlmostEqual(vec3.max(), 0.4)

        vec1.max(vec2)
        self.assertAlmostEqual(vec1, Vector2d(0.3, 0.5))

        vec1.max(vec3)
        self.assertAlmostEqual(vec1, Vector2d(0.4, 0.5))

    def test_min(self):
        vec1 = Vector2d(0.3, 0.5)
        vec2 = Vector2d(0.1, 0.2)
        vec3 = Vector2d(0.05, 0.1)

        self.assertAlmostEqual(vec1.min(), 0.3)
        self.assertAlmostEqual(vec3.min(), 0.05)

        vec1.min(vec2)
        self.assertAlmostEqual(vec1, Vector2d(0.1, 0.2))

        vec1.min(vec3)
        self.assertAlmostEqual(vec1, Vector2d(0.05, 0.1))

    def test_equal_tolerance(self):
        # Test Equal function with specified tolerance
        self.assertFalse(Vector2d.ZERO.equal(Vector2d.ONE, 1e-6))
        self.assertFalse(Vector2d.ZERO.equal(Vector2d.ONE, 1e-3))
        self.assertFalse(Vector2d.ZERO.equal(Vector2d.ONE, 1e-1))
        self.assertTrue(Vector2d.ZERO.equal(Vector2d.ONE, 1))
        self.assertTrue(Vector2d.ZERO.equal(Vector2d.ONE, 1.1))

    def test_dot(self):
        v = Vector2d(1, 2)
        self.assertAlmostEqual(v.dot(Vector2d(3, 4)), 11.0)
        self.assertAlmostEqual(v.dot(Vector2d(0, 0)), 0.0)
        self.assertAlmostEqual(v.dot(Vector2d(1, 0)), 1.0)
        self.assertAlmostEqual(v.dot(Vector2d(0, 1)), 2.0)

    def test_correct(self):
        vec1 = Vector2d(0, float("nan"))
        vec2 = Vector2d(float("inf"), -1)
        vec3 = Vector2d(10, -2)

        vec1.correct()
        vec2.correct()
        vec3.correct()

        self.assertAlmostEqual(vec1, Vector2d(0, 0))
        self.assertAlmostEqual(vec2, Vector2d(0, -1))
        self.assertAlmostEqual(vec3, Vector2d(10, -2))

    def test_abs_dot(self):
        v = Vector2d(1, -2)

        self.assertAlmostEqual(v.abs_dot(Vector2d(3, 4)), 11.0)
        self.assertAlmostEqual(v.abs_dot(Vector2d(0, 0)), 0.0)
        self.assertAlmostEqual(v.abs_dot(Vector2d(1, 0)), 1.0)
        self.assertAlmostEqual(v.abs_dot(Vector2d(0, 1)), 2.0)

    def test_add(self):
        vec1 = Vector2d(0.1, 0.2)
        vec2 = Vector2d(1.1, 2.2)

        vec3 = vec1
        vec3 += vec2

        self.assertAlmostEqual(vec1 + vec2, Vector2d(1.2, 2.4))
        self.assertAlmostEqual(vec3, Vector2d(1.2, 2.4))

        # Add zero
        # Scalar right
        self.assertEqual(vec1 + 0, vec1)

        # Vector left and right
        self.assertAlmostEqual(Vector2d.ZERO + vec1, vec1)
        self.assertAlmostEqual(vec1 + Vector2d.ZERO, vec1)

        # Addition assigment
        vec4 = Vector2d(vec1)
        vec4 += 0
        self.assertEqual(vec4, vec1)
        vec4 += Vector2d.ZERO
        self.assertAlmostEqual(vec4, vec1)

        # Add non-trivial scalar values left and right
        self.assertEqual(vec1 + 2.5, Vector2d(2.6, 2.7))

        vec1 = vec4
        vec4 += 2.5
        self.assertEqual(vec4, Vector2d(2.6, 2.7))

    def test_sub(self):
        vec1 = Vector2d(0.1, 0.2)
        vec2 = Vector2d(1.1, 2.2)

        vec3 = vec2
        vec3 -= vec1

        self.assertAlmostEqual(vec2 - vec1, Vector2d(1.0, 2.0))
        self.assertAlmostEqual(vec3, Vector2d(1.0, 2.0))

        # Subtraction with zeros
        # Scalar right
        self.assertEqual(vec1 - 0, vec1)

        # Vector left and right
        self.assertAlmostEqual(Vector2d.ZERO - vec1, -vec1)
        self.assertAlmostEqual(vec1 - Vector2d.ZERO, vec1)

        # Subtraction assignment
        vec4 = Vector2d(vec1)
        vec4 -= 0
        self.assertEqual(vec4, vec1)
        vec4 -= Vector2d.ZERO
        self.assertAlmostEqual(vec4, vec1)

        # Subtract non-trivial scalar values left and right
        self.assertEqual(vec1 - 2.5, -Vector2d(2.4, 2.3))

        vec4 = vec1
        vec4 -= 2.5
        self.assertEqual(vec4, -Vector2d(2.4, 2.3))

    def test_multiply(self):
        v = Vector2d(0.1, -4.2)

        vec2 = v * 2.0
        self.assertEqual(vec2, Vector2d(0.2, -8.4))

        vec2 *= 4.0
        self.assertEqual(vec2, Vector2d(0.8, -33.6))

        # Multiply by zero
        # Scalar right
        self.assertEqual(v * 0, Vector2d.ZERO)

        # Element-wise vector multiplication
        self.assertEqual(v * Vector2d.ZERO, Vector2d.ZERO)

        # Multiply by one
        # Scalar right
        self.assertEqual(v * 1, v)

        # Element-wise vector multiplication
        self.assertEqual(v * Vector2d.ONE, v)

        # Multiply by non-trivial scalar value
        scalar = 2.5
        expect = Vector2d(0.25, -10.5)
        self.assertEqual(v * scalar, expect)

        # Multiply by itself element-wise
        v.set(0.1, 0.5)
        self.assertAlmostEqual(v * v, Vector2d(0.01, 0.25))

    def test_lenght(self):
        # Zero vector
        self.assertAlmostEqual(Vector2d.ZERO.length(), 0.0)
        self.assertAlmostEqual(Vector2d.ZERO.squared_length(), 0.0)

        # One vector
        self.assertAlmostEqual(Vector2d.ONE.length(),
                               math.sqrt(2), delta=1e-10)
        self.assertAlmostEqual(Vector2d.ONE.squared_length(), 2.0)

        # Arbitrary vector
        v = Vector2d(0.1, -4.2)
        self.assertAlmostEqual(v.length(), 4.20119030752, delta=1e-10)
        self.assertAlmostEqual(v.squared_length(), 17.65)

        # Integer vector
        v = Vector2d(3, 4)
        self.assertAlmostEqual(v.length(), 5)
        self.assertAlmostEqual(v.squared_length(), 25)

    def test_nan(self):
        nanVec = Vector2d.NAN
        self.assertFalse(nanVec.is_finite())
        self.assertTrue(math.isnan(nanVec.x()))
        self.assertTrue(math.isnan(nanVec.y()))

        nanVec.correct()
        self.assertEqual(Vector2d.ZERO, nanVec)
        self.assertTrue(nanVec.is_finite())

        nanVecF = Vector2f.NAN
        self.assertFalse(nanVecF.is_finite())
        self.assertTrue(math.isnan(nanVecF.x()))
        self.assertTrue(math.isnan(nanVecF.y()))

        nanVecF.correct()
        self.assertEqual(Vector2f.ZERO, nanVecF)
        self.assertTrue(nanVecF.is_finite())

if __name__ == '__main__':
    unittest.main()

[(319, 'unittest.main', 'unittest.main', ([], {}), False, 'import unittest\n'), (24, 'ignition.math.Vector2d', 'Vector2d', ([], {}), False, 'from ignition.math import Vector2d\n'), (28, 'ignition.math.Vector2d', 'Vector2d', (['(1)', '(0)'], {}), False, 'from ignition.math import Vector2d\n'), (32, 'ignition.math.Vector2d', 'Vector2d', (['vec'], {}), False, 'from ignition.math import Vector2d\n'), (40, 'ignition.math.Vector2d', 'Vector2d', ([], {}), False, 'from ignition.math import Vector2d\n'), (44, 'ignition.math.Vector2d', 'Vector2d', (['(1)', '(2)'], {}), False, 'from ignition.math import Vector2d\n'), (62, 'ignition.math.Vector2d', 'Vector2d', (['(6)', '(7)'], {}), False, 'from ignition.math import Vector2d\n'), (69, 'ignition.math.Vector2d', 'Vector2d', (['(5)', '(6)'], {}), False, 'from ignition.math import Vector2d\n'), (77, 'ignition.math.Vector2d', 'Vector2d', (['(1)', '(6)'], {}), False, 'from ignition.math import Vector2d\n'), (86, 'ignition.math.Vector2d', 'Vector2d', (['(2)', '(3)'], {}), False, 'from ignition.math import Vector2d\n'), (104, 'ignition.math.Vector2d', 'Vector2d', (['(2)', '(4)'], {}), False, 'from ignition.math import Vector2d\n'), (120, 'ignition.math.Vector2d', 'Vector2d', (['(0.1)', '(0.2)'], {}), False, 'from ignition.math import Vector2d\n'), (121, 'ignition.math.Vector2d', 'Vector2d', (['(0.3)', '(0.5)'], {}), False, 'from ignition.math import Vector2d\n'), (122, 'ignition.math.Vector2d', 'Vector2d', (['(0.4)', '(0.2)'], {}), False, 'from ignition.math import Vector2d\n'), (134, 'ignition.math.Vector2d', 'Vector2d', (['(0.3)', '(0.5)'], {}), False, 'from ignition.math import Vector2d\n'), (135, 'ignition.math.Vector2d', 'Vector2d', (['(0.1)', '(0.2)'], {}), False, 'from ignition.math import Vector2d\n'), (136, 'ignition.math.Vector2d', 'Vector2d', (['(0.05)', '(0.1)'], {}), False, 'from ignition.math import Vector2d\n'), (156, 'ignition.math.Vector2d', 'Vector2d', (['(1)', '(2)'], {}), False, 'from ignition.math import Vector2d\n'), (165, 'ignition.math.Vector2d', 'Vector2d', (['(10)', '(-2)'], {}), False, 'from ignition.math import Vector2d\n'), (176, 'ignition.math.Vector2d', 'Vector2d', (['(1)', '(-2)'], {}), False, 'from ignition.math import Vector2d\n'), (184, 'ignition.math.Vector2d', 'Vector2d', (['(0.1)', '(0.2)'], {}), False, 'from ignition.math import Vector2d\n'), (185, 'ignition.math.Vector2d', 'Vector2d', (['(1.1)', '(2.2)'], {}), False, 'from ignition.math import Vector2d\n'), (202, 'ignition.math.Vector2d', 'Vector2d', (['vec1'], {}), False, 'from ignition.math import Vector2d\n'), (216, 'ignition.math.Vector2d', 'Vector2d', (['(0.1)', '(0.2)'], {}), False, 'from ignition.math import Vector2d\n'), (217, 'ignition.math.Vector2d', 'Vector2d', (['(1.1)', '(2.2)'], {}), False, 'from ignition.math import Vector2d\n'), (234, 'ignition.math.Vector2d', 'Vector2d', (['vec1'], {}), False, 'from ignition.math import Vector2d\n'), (248, 'ignition.math.Vector2d', 'Vector2d', (['(0.1)', '(-4.2)'], {}), False, 'from ignition.math import Vector2d\n'), (272, 'ignition.math.Vector2d', 'Vector2d', (['(0.25)', '(-10.5)'], {}), False, 'from ignition.math import Vector2d\n'), (290, 'ignition.math.Vector2d', 'Vector2d', (['(0.1)', '(-4.2)'], {}), False, 'from ignition.math import Vector2d\n'), (295, 'ignition.math.Vector2d', 'Vector2d', (['(3)', '(4)'], {}), False, 'from ignition.math import Vector2d\n'), (66, 'ignition.math.Vector2d', 'Vector2d', (['(1)', '(2)'], {}), False, 'from ignition.math import Vector2d\n'), (73, 'ignition.math.Vector2d', 'Vector2d', (['(2)', '(4)'], {}), False, 'from ignition.math import Vector2d\n'), (82, 'ignition.math.Vector2d', 'Vector2d', (['(2)', '(3)'], {}), False, 'from ignition.math import Vector2d\n'), (100, 'ignition.math.Vector2d', 'Vector2d', (['(2)', '(4)'], {}), False, 'from ignition.math import Vector2d\n'), (128, 'ignition.math.Vector2d', 'Vector2d', (['(0.3)', '(0.5)'], {}), False, 'from ignition.math import Vector2d\n'), (131, 'ignition.math.Vector2d', 'Vector2d', (['(0.4)', '(0.5)'], {}), False, 'from ignition.math import Vector2d\n'), (142, 'ignition.math.Vector2d', 'Vector2d', (['(0.1)', '(0.2)'], {}), False, 'from ignition.math import Vector2d\n'), (145, 'ignition.math.Vector2d', 'Vector2d', (['(0.05)', '(0.1)'], {}), False, 'from ignition.math import Vector2d\n'), (149, 'ignition.math.Vector2d.ZERO.equal', 'Vector2d.ZERO.equal', (['Vector2d.ONE', '(1e-06)'], {}), False, 'from ignition.math import Vector2d\n'), (150, 'ignition.math.Vector2d.ZERO.equal', 'Vector2d.ZERO.equal', (['Vector2d.ONE', '(0.001)'], {}), False, 'from ignition.math import Vector2d\n'), (151, 'ignition.math.Vector2d.ZERO.equal', 'Vector2d.ZERO.equal', (['Vector2d.ONE', '(0.1)'], {}), False, 'from ignition.math import Vector2d\n'), (152, 'ignition.math.Vector2d.ZERO.equal', 'Vector2d.ZERO.equal', (['Vector2d.ONE', '(1)'], {}), False, 'from ignition.math import Vector2d\n'), (153, 'ignition.math.Vector2d.ZERO.equal', 'Vector2d.ZERO.equal', (['Vector2d.ONE', '(1.1)'], {}), False, 'from ignition.math import Vector2d\n'), (171, 'ignition.math.Vector2d', 'Vector2d', (['(0)', '(0)'], {}), False, 'from ignition.math import Vector2d\n'), (172, 'ignition.math.Vector2d', 'Vector2d', (['(0)', '(-1)'], {}), False, 'from ignition.math import Vector2d\n'), (173, 'ignition.math.Vector2d', 'Vector2d', (['(10)', '(-2)'], {}), False, 'from ignition.math import Vector2d\n'), (190, 'ignition.math.Vector2d', 'Vector2d', (['(1.2)', '(2.4)'], {}), False, 'from ignition.math import Vector2d\n'), (191, 'ignition.math.Vector2d', 'Vector2d', (['(1.2)', '(2.4)'], {}), False, 'from ignition.math import Vector2d\n'), (209, 'ignition.math.Vector2d', 'Vector2d', (['(2.6)', '(2.7)'], {}), False, 'from ignition.math import Vector2d\n'), (213, 'ignition.math.Vector2d', 'Vector2d', (['(2.6)', '(2.7)'], {}), False, 'from ignition.math import Vector2d\n'), (222, 'ignition.math.Vector2d', 'Vector2d', (['(1.0)', '(2.0)'], {}), False, 'from ignition.math import Vector2d\n'), (223, 'ignition.math.Vector2d', 'Vector2d', (['(1.0)', '(2.0)'], {}), False, 'from ignition.math import Vector2d\n'), (251, 'ignition.math.Vector2d', 'Vector2d', (['(0.2)', '(-8.4)'], {}), False, 'from ignition.math import Vector2d\n'), (254, 'ignition.math.Vector2d', 'Vector2d', (['(0.8)', '(-33.6)'], {}), False, 'from ignition.math import Vector2d\n'), (277, 'ignition.math.Vector2d', 'Vector2d', (['(0.01)', '(0.25)'], {}), False, 'from ignition.math import Vector2d\n'), (281, 'ignition.math.Vector2d.ZERO.length', 'Vector2d.ZERO.length', ([], {}), False, 'from ignition.math import Vector2d\n'), (282, 'ignition.math.Vector2d.ZERO.squared_length', 'Vector2d.ZERO.squared_length', ([], {}), False, 'from ignition.math import Vector2d\n'), (285, 'ignition.math.Vector2d.ONE.length', 'Vector2d.ONE.length', ([], {}), False, 'from ignition.math import Vector2d\n'), (286, 'math.sqrt', 'math.sqrt', (['(2)'], {}), False, 'import math\n'), (287, 'ignition.math.Vector2d.ONE.squared_length', 'Vector2d.ONE.squared_length', ([], {}), False, 'from ignition.math import Vector2d\n'), (47, 'ignition.math.Vector2d', 'Vector2d', ([], {}), False, 'from ignition.math import Vector2d\n'), (51, 'ignition.math.Vector2d', 'Vector2d', (['(0.447214)', '(0.894427)'], {}), False, 'from ignition.math import Vector2d\n'), (55, 'ignition.math.Vector2d', 'Vector2d', (['(4)', '(5)'], {}), False, 'from ignition.math import Vector2d\n'), (59, 'ignition.math.Vector2d', 'Vector2d', (['(1)', '(2)'], {}), False, 'from ignition.math import Vector2d\n'), (63, 'ignition.math.Vector2d', 'Vector2d', (['(6)', '(7)'], {}), False, 'from ignition.math import Vector2d\n'), (67, 'ignition.math.Vector2d', 'Vector2d', (['(7)', '(9)'], {}), False, 'from ignition.math import Vector2d\n'), (70, 'ignition.math.Vector2d', 'Vector2d', (['(12)', '(15)'], {}), False, 'from ignition.math import Vector2d\n'), (74, 'ignition.math.Vector2d', 'Vector2d', (['(10)', '(11)'], {}), False, 'from ignition.math import Vector2d\n'), (78, 'ignition.math.Vector2d', 'Vector2d', (['(1)', '(-2)'], {}), False, 'from ignition.math import Vector2d\n'), (83, 'ignition.math.Vector2d', 'Vector2d', (['(5)', '(2)'], {}), False, 'from ignition.math import Vector2d\n'), (87, 'ignition.math.Vector2d', 'Vector2d', (['(5)', '(2)'], {}), False, 'from ignition.math import Vector2d\n'), (92, 'ignition.math.Vector2d', 'Vector2d', (['(5)', '(3)'], {}), False, 'from ignition.math import Vector2d\n'), (96, 'ignition.math.Vector2d', 'Vector2d', (['(5)', '(3)'], {}), False, 'from ignition.math import Vector2d\n'), (101, 'ignition.math.Vector2d', 'Vector2d', (['(20)', '(24)'], {}), False, 'from ignition.math import Vector2d\n'), (105, 'ignition.math.Vector2d', 'Vector2d', (['(20)', '(24)'], {}), False, 'from ignition.math import Vector2d\n'), (110, 'ignition.math.Vector2d', 'Vector2d', (['(20)', '(12)'], {}), False, 'from ignition.math import Vector2d\n'), (114, 'ignition.math.Vector2d', 'Vector2d', (['(20)', '(12)'], {}), False, 'from ignition.math import Vector2d\n'), (157, 'ignition.math.Vector2d', 'Vector2d', (['(3)', '(4)'], {}), False, 'from ignition.math import Vector2d\n'), (158, 'ignition.math.Vector2d', 'Vector2d', (['(0)', '(0)'], {}), False, 'from ignition.math import Vector2d\n'), (159, 'ignition.math.Vector2d', 'Vector2d', (['(1)', '(0)'], {}), False, 'from ignition.math import Vector2d\n'), (160, 'ignition.math.Vector2d', 'Vector2d', (['(0)', '(1)'], {}), False, 'from ignition.math import Vector2d\n'), (178, 'ignition.math.Vector2d', 'Vector2d', (['(3)', '(4)'], {}), False, 'from ignition.math import Vector2d\n'), (179, 'ignition.math.Vector2d', 'Vector2d', (['(0)', '(0)'], {}), False, 'from ignition.math import Vector2d\n'), (180, 'ignition.math.Vector2d', 'Vector2d', (['(1)', '(0)'], {}), False, 'from ignition.math import Vector2d\n'), (181, 'ignition.math.Vector2d', 'Vector2d', (['(0)', '(1)'], {}), False, 'from ignition.math import Vector2d\n'), (241, 'ignition.math.Vector2d', 'Vector2d', (['(2.4)', '(2.3)'], {}), False, 'from ignition.math import Vector2d\n'), (245, 'ignition.math.Vector2d', 'Vector2d', (['(2.4)', '(2.3)'], {}), False, 'from ignition.math import Vector2d\n')]