"""
@author: Peter Corke
"""

from roboticstoolbox import DHRobot, RevoluteDH
from math import pi
from spatialmath import SE3
import numpy as np


class TwoLink(DHRobot):
    """
    Class that models a 2-link robot moving in the vertical plane

    :param symbolic: use symbolic constants
    :type symbolic: bool

    ``TwoLink()`` is a class which models a 2-link planar robot and
    describes its kinematic and dynamic characteristics using standard DH
    conventions. of a simple planar 2-link mechanism moving in the xz-plane, it experiences gravity loading.
    All mass is concentrated at the joints.

    .. runblock:: pycon

        >>> import roboticstoolbox as rtb
        >>> robot = rtb.models.DH.TwoLink()
        >>> print(robot)

    The parameters values depend on the ``symbolic`` parameter

    =======================================  =================  ==============
    Parameters                               Numeric values     Symbolic values
    =======================================  =================  ==============
    link lengths                             1, 1               a1, a2
    link masses                              1, 1               m1, m2
    link CoMs in the link frame x-direction  -0.5, -0.5         c1, c2
    gravitational acceleration               9.8                g
    =======================================  =================  ==============

    Defined joint configurations are:

        - qz, zero angles, all folded up
        - q1, links are horizontal and vertical respectively
        - q2, links are vertical and horizontal respectively
        - qn, nominal working configuration

    .. note::

        - Robot has only 2 DoF.
        - Motor inertia is 0.
        - Link inertias are 0.
        - Viscous and Coulomb friction is 0.

    :Reference: Based on Fig 3-6 (p73) of Spong and Vidyasagar (1st edition).

    .. codeauthor:: Peter Corke
    """

    def __init__(self, symbolic=False):

        if symbolic:
            import spatialmath.base.symbolic as sym

            zero = sym.zero()
            pi = sym.pi()
            a1, a2 = sym.symbol("a1 a2")  # type: ignore
            m1, m2 = sym.symbol("m1 m2")  # type: ignore
            c1, c2 = sym.symbol("c1 c2")  # type: ignore
            g = sym.symbol("g")
        else:
            from math import pi

            zero = 0.0
            a1 = 1
            a2 = 1
            m1 = 1
            m2 = 1
            c1 = -0.5
            c2 = -0.5
            g = 9.8

        links = [
            RevoluteDH(a=a1, alpha=zero, m=m1, r=[c1, 0, 0]),
            RevoluteDH(a=a2, alpha=zero, m=m2, r=[c2, 0, 0]),
        ]

        super().__init__(
            links, symbolic=symbolic, name="2 link", keywords=("planar", "dynamics")
        )

        self.qr = np.array([pi / 6, -pi / 6])
        self.qz = np.zeros(2)

        self.addconfiguration("qr", self.qr)
        self.addconfiguration("qz", self.qz)

        self.addconfiguration_attr("qz", [0, 0])
        self.addconfiguration_attr("q1", [0, pi / 2])
        self.addconfiguration_attr("q2", [pi / 2, -pi / 2])
        self.addconfiguration_attr("qn", [pi / 6, -pi / 6])

        self.base = SE3.Rx(pi / 2)
        self.gravity = [0, 0, g]


if __name__ == "__main__":  # pragma nocover

    robot = TwoLink(symbolic=True)
    print(robot)
