import unittest
import cadquery as Cq
import nhf.touhou.houjuu_nue as M
import nhf.touhou.houjuu_nue.joints as MJ
import nhf.touhou.houjuu_nue.electronics as ME
from nhf.checks import pairwise_intersection

class TestElectronics(unittest.TestCase):

    def test_flexor(self):
        flexor = ME.Flexor(
            motion_span=60,
        )
        self.assertAlmostEqual(
            flexor.target_length_at_angle(0),
            flexor.actuator.stroke_length + flexor.actuator.conn_length)
        self.assertAlmostEqual(
            flexor.target_length_at_angle(flexor.motion_span),
            flexor.actuator.conn_length)


class TestJoints(unittest.TestCase):

    def test_shoulder_collision_of_torsion_joint(self):
        j = MJ.ShoulderJoint()
        assembly = j.torsion_joint.rider_track_assembly()
        self.assertEqual(pairwise_intersection(assembly), [])

    def test_shoulder_collision_0(self):
        j = MJ.ShoulderJoint()
        assembly = j.assembly()
        self.assertEqual(pairwise_intersection(assembly), [])

    def test_shoulder_align(self):
        j = MJ.ShoulderJoint()
        a = j.assembly()
        l_t_c0 = a.get_abs_location("parent_top/lip?conn0")
        l_b_c0 = a.get_abs_location("parent_bot/lip?conn0")
        v = l_t_c0 - l_b_c0
        self.assertAlmostEqual(v.x, 0)
        self.assertAlmostEqual(v.y, 0)

    def test_shoulder_joint_dist(self):
        """
        Tests the arm radius
        """
        j = MJ.ShoulderJoint()
        for deflection in [0, 40, j.angle_max_deflection]:
            with self.subTest(deflection=deflection):
                a = j.assembly(deflection=deflection)
                # Axle
                o = a.get_abs_location("parent_top/track?spring")
                l_c1 = a.get_abs_location("parent_top/lip?conn0")
                l_c2= a.get_abs_location("parent_top/lip?conn1")
                v_c = 0.5 * ((l_c1 - o) + (l_c2 - o))
                v_c.z = 0
                self.assertAlmostEqual(v_c.Length, j.parent_lip_ext)

    def test_disk_collision_0(self):
        j = MJ.DiskJoint()
        assembly = j.assembly(angle=0)
        self.assertEqual(pairwise_intersection(assembly), [])
    def test_disk_collision_mid(self):
        j = MJ.DiskJoint()
        assembly = j.assembly(angle=j.movement_angle / 2)
        self.assertEqual(pairwise_intersection(assembly), [])
    def test_disk_collision_max(self):
        j = MJ.DiskJoint()
        assembly = j.assembly(angle=j.movement_angle)
        self.assertEqual(pairwise_intersection(assembly), [])

    def test_elbow_joint_dist(self):
        """
        Tests the arm radius
        """
        j = MJ.ElbowJoint()
        for angle in [0, 10, 20, j.disk_joint.movement_angle]:
            with self.subTest(angle=angle):
                a = j.assembly(angle=angle)
                o = a.get_abs_location("child/disk?mate_bot")
                l_c1 = a.get_abs_location("child/lip?conn_top0")
                l_c2 = a.get_abs_location("child/lip?conn_bot0")
                v_c = 0.5 * ((l_c1 - o) + (l_c2 - o))
                v_c.z = 0
                self.assertAlmostEqual(v_c.Length, j.child_arm_radius)

                l_p1 = a.get_abs_location("parent_upper/lip?conn_top0")
                l_p2 = a.get_abs_location("parent_upper/lip?conn_bot0")
                v_p = 0.5 * ((l_p1 - o) + (l_p2 - o))
                v_p.z = 0
                self.assertAlmostEqual(v_p.Length, j.parent_arm_radius)


class Test(unittest.TestCase):

    def test_hs_joint_parent(self):
        p = M.Parameters()
        obj = p.harness.hs_joint_parent()
        self.assertIsInstance(obj.val().solids(), Cq.Solid, msg="H-S joint must be in one piece")

    def test_wings_assembly(self):
        p = M.Parameters()
        p.wings_harness_assembly()
    def test_trident_assembly(self):
        p = M.Parameters()
        assembly = p.trident.assembly()
        bbox = assembly.toCompound().BoundingBox()
        length = bbox.zlen
        self.assertGreater(length, 1300)
        self.assertLess(length, 1700)
    #def test_assemblies(self):
    #    p = M.Parameters()
    #    p.check_all()

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