# coding: UTF-8

import rospy
import math
import copy

from python_qt_binding.QtCore import Qt, QPointF, QRectF
from python_qt_binding.QtGui import QPainter, QPen ,QColor, QPolygonF
from python_qt_binding.QtGui import QMouseEvent
from python_qt_binding.QtWidgets import QWidget

from geometry_msgs.msg import Pose2D

from consai2_msgs.msg import VisionGeometry, BallInfo, RobotInfo
from consai2_msgs.msg import Replacements, ReplaceBall, ReplaceRobot
from consai2_msgs.msg import ControlTarget
from consai2_msgs.msg import DecodedReferee

import tool


class PaintWidget(QWidget):
    def __init__(self, parent=None):
        super(PaintWidget, self).__init__(parent)

        self._WHITE_LINE_THICKNESS = 2 # 白線の太さ
        self._ZOOM_RATE = 0.1 # 拡大縮小率
        self._SCALE_LIMIT = 0.2 # 縮小率の限界値
        self._ALPHA_DETECTED = 255 # 検出できたロボット・ボールの透明度
        self._ALPHA_NOT_DETECTED  = 127 # 検出できなかったロボット・ボールの透明度
        self._COLOR_BALL = QColor(Qt.red)
        self._COLOR_ROBOT  = {'blue':QColor(Qt.cyan), 'yellow':QColor(Qt.yellow)}
        self._ID_POS = (0.15, 0.15) # IDを描画するロボット中心からの位置
        self._FLAG_POS = (0.15, 0) # control_targetのフラグを描画するロボット中心からの位置

        # Replace
        self._REPLACE_CLICK_POS_THRESHOLD = 0.1
        self._REPLACE_CLICK_VEL_ANGLE_THRESHOLD = self._REPLACE_CLICK_POS_THRESHOLD + 0.1
        self._REPLACE_BALL_VELOCITY_GAIN = 3.0
        self._REPLACE_MAX_BALL_VELOCITY = 8.0

        self._BALL_RADIUS = rospy.get_param('consai2_description/ball_radius', 0.0215)
        self._ROBOT_RADIUS = rospy.get_param('consai2_description/robot_radius', 0.09)
        self._MAX_ID = rospy.get_param('consai2_description/max_id', 15)
        self._SIDE = rospy.get_param('consai2_description/our_side', 'left')

        # チームサイドの反転
        self._invert_side = False
        if self._SIDE != 'left':
            self._invert_side = True

        # GUIパラメータ
        self._trans      = QPointF(0.0, 0.0) # x, y方向の移動
        self._mouse_trans = QPointF(0.0, 0.0) # マウス操作による移動
        self._scale      = QPointF(1.0, 1.0) # 拡大, 縮小
        self._do_rotate_view = False # 90度回転判定
        self._view_height = self.height() # 描画サイズ（縦）
        self._view_width = self.width() # 描画サイズ（横）
        self._scale_field_to_view = 1.0 # フィールドから描画領域に縮小するスケール
        self._click_point = QPointF(0.0, 0.0) # マウスでクリックした描画座標
        self._current_mouse_pos = QPointF(0.0, 0.0) # マウスカーソル位置
        self._replace_func = None
        self._replace_id = 0
        self._replace_is_yellow = False
        self._do_replacement = False
        self._replacement_target = {'ball_pos':False, 'ball_vel':False,
                'robot_pos':False, 'robot_angle':False}

        # フィールド形状
        # raw_vision_geometryの値で更新される
        self._field_length = 9.0
        self._field_width = 6.0
        self._field_goal_width = 1.0
        self._field_goal_depth = 0.2
        self._field_boundary_width = 0.3
        self._field_lines = []
        self._field_arcs = []

        # ジョイスティック情報
        self._joy_target = ControlTarget()

        # ロボット・ボール情報
        self._ball_info = BallInfo()
        self._robot_info = {'blue':[],'yellow':[]}

        # レフェリー情報
        self._decoded_referee = None

        # Publisher
        self._pub_replace = rospy.Publisher('sim_sender/replacements', 
                Replacements, queue_size=1)

        # Subscribers
        self._sub_decoded_referee = rospy.Subscriber(
                'referee_wrapper/decoded_referee', DecodedReferee, 
                self._callback_referee, queue_size=1)

        self._sub_geometry = rospy.Subscriber(
                'vision_receiver/raw_vision_geometry', VisionGeometry, 
                self._callback_geometry, queue_size=1)

        self._sub_ball_info = rospy.Subscriber(
                'vision_wrapper/ball_info', BallInfo,
                self._callback_ball_info, queue_size=1)

        self._sub_joy_target = rospy.Subscriber(
                'consai2_examples/joy_target', ControlTarget, 
                self._callback_joy_target, queue_size=1)

        self._subs_robot_info = {'blue':[], 'yellow':[]}

        self._control_targets = {'blue':[], 'yellow':[]}
        self._subs_control_target = {'blue':[], 'yellow':[]}

        for robot_id in range(self._MAX_ID +1):
            self._robot_info['blue'].append(RobotInfo())
            self._robot_info['yellow'].append(RobotInfo())
            self._control_targets['blue'].append(ControlTarget())
            self._control_targets['yellow'].append(ControlTarget())

            # 末尾に16進数の文字列をつける
            topic_id = hex(robot_id)[2:]
            topic_name = 'vision_wrapper/robot_info_blue_' + topic_id
            self._subs_robot_info['blue'].append(
                    rospy.Subscriber(topic_name, RobotInfo,
                        self._callback_blue_info, callback_args=robot_id))

            topic_name = 'vision_wrapper/robot_info_yellow_' + topic_id
            self._subs_robot_info['yellow'].append(
                    rospy.Subscriber(topic_name, RobotInfo,
                        self._callback_yellow_info, callback_args=robot_id))

            topic_name = 'consai2_game/control_target_blue_' + topic_id
            self._subs_control_target['blue'].append(
                    rospy.Subscriber(topic_name, ControlTarget,
                        self._callback_blue_target, callback_args=robot_id))
            
            topic_name = 'consai2_game/control_target_blue_' + topic_id
            self._subs_control_target['blue'].append(
                    rospy.Subscriber(topic_name, ControlTarget,
                        self._callback_blue_target, callback_args=robot_id))

            topic_name = 'consai2_game/control_target_yellow_' + topic_id
            self._subs_control_target['yellow'].append(
                    rospy.Subscriber(topic_name, ControlTarget,
                        self._callback_yellow_target, callback_args=robot_id))


        # Configs
        # This function enables mouse tracking without pressing mouse button
        self.setMouseTracking(True)

    def _callback_geometry(self, msg):
        # フィールド形状を更新
        if msg.field_length:
            self._field_length = msg.field_length

        if msg.field_width:
            self._field_width = msg.field_width

        if msg.goal_width:
            self._field_goal_width = msg.goal_width

        if msg.goal_depth:
            self._field_goal_depth = msg.goal_depth

        if msg.boundary_width:
            self._field_boundary_width = msg.boundary_width

        if msg.field_lines:
            self._field_lines = []
            for line in msg.field_lines:
                self._field_lines.append(
                        {"name":line.name, "p1_x":line.p1_x, "p1_y":line.p1_y,
                            "p2_x":line.p2_x, "p2_y":line.p2_y, "thickness":line.thickness})
        if msg.field_arcs:
            self._field_arcs = []
            for arc in msg.field_arcs:
                self._field_arcs.append(
                    {"name":arc.name, "center_x":arc.center_x, "center_y":arc.center_y,
                        "radius":arc.radius, "a1":arc.a1, "a2":arc.a2, "thickness":arc.thickness})

        self._resize_draw_world()

    def _callback_referee(self, msg):
        self._decoded_referee = msg

    def _callback_ball_info(self, msg):
        self._ball_info = msg

    def _callback_blue_info(self, msg, robot_id):
        self._robot_info['blue'][robot_id] = msg

    def _callback_yellow_info(self, msg, robot_id):
        self._robot_info['yellow'][robot_id] = msg

    def _callback_blue_target(self, msg, robot_id):
        self._control_targets['blue'][robot_id] = msg

    def _callback_yellow_target(self, msg, robot_id):
        self._control_targets['yellow'][robot_id] = msg

    def _callback_joy_target(self, msg):
        self._joy_target = msg


    def mousePressEvent(self, event):
        # マウスのドラッグ操作で描画領域を移動する
        # 右クリックで移動と拡大縮小をリセットする
        if event.buttons() == Qt.LeftButton:
            self._click_point = event.localPos()

            self._do_replacement = self._is_replacement_click(self._click_point)

        elif event.buttons() == Qt.RightButton:
            self._reset_painter_status()

        self.update()


    def mouseMoveEvent(self, event):
        # マウスのドラッグ操作で描画領域を移動する
        # Replacementを行うときは描画領域を移動しない
        self._current_mouse_pos = event.localPos()

        if self._do_replacement:
            pass
        elif event.buttons() == Qt.LeftButton:
            self._mouse_trans = (
                    self._current_mouse_pos - self._click_point) / self._scale.x()

        self.update()


    def mouseReleaseEvent(self, event):
        # マウスのドラッグ操作で描画領域を移動する
        # Replacementを行うときは描画領域を移動しない
        if self._do_replacement:
            self._do_replacement = False
            self._replace_func(event.localPos())
        else:
            self._trans += self._mouse_trans
            self._mouse_trans = QPointF(0.0, 0.0)
        
        self.update()


    def wheelEvent(self, event):
        # マウスのホイール操作で描画領域を拡大縮小する
        s = self._scale.x()
        if event.angleDelta().y() > 0:
            self._scale.setX(s + self._ZOOM_RATE)
            self._scale.setY(s + self._ZOOM_RATE)
        else:
            if s > self._SCALE_LIMIT:
                self._scale.setX(s - self._ZOOM_RATE)
                self._scale.setY(s - self._ZOOM_RATE)

        self.update()


    def paintEvent(self, event):
        painter = QPainter(self)

        # 描画の中心をWidgetの中心に持ってくる
        cx = float(self.width()) * 0.5
        cy = float(self.height()) * 0.5
        painter.translate(cx,cy)

        painter.scale(self._scale.x(), self._scale.y())
        painter.translate(self._trans + self._mouse_trans)

        if self._do_rotate_view is True:
            painter.rotate(-90)

        # これ以降に書きたいものを重ねる
        self._draw_field(painter)

        # Referee情報
        self._draw_referee(painter)

        self._draw_ball(painter)
        self._draw_ball_velocity(painter)

        # JoyStick関連
        if len(self._joy_target.path) > 0:
            self._draw_joy_target(painter)

        self._draw_robots(painter)


        # grSim Replacement関連
        if self._replacement_target['ball_pos'] or self._replacement_target['robot_pos']:
            self._draw_pos_replacement(painter)
            self._draw_cursor_coordinate(painter)

        elif self._replacement_target['ball_vel']:
            self._draw_vel_replacement(painter)

        elif self._replacement_target['robot_angle']:
            self._draw_angle_replacement(painter)

        else:
            self._draw_cursor_coordinate(painter)



    def resizeEvent(self, event):
        self._resize_draw_world()


    def _resize_draw_world(self):
        # Widgetのサイズに合わせて、描くフィールドのサイズを変える
        # 描画の回転判断もしてくれるすぐれもの

        # Widgetの縦横比を算出
        widget_height = float(self.height())
        widget_width = float(self.width())
        widget_w_per_h = widget_width / widget_height

        # Fieldの縦横比を算出
        field_width = self._field_length + self._field_boundary_width * 2.0
        field_height = self._field_width + self._field_boundary_width * 2.0
        field_w_per_h = field_width / field_height
        field_h_per_w = 1.0 / field_w_per_h

        if widget_w_per_h >= field_w_per_h:
            # Widgetが横長のとき
            self._view_height = widget_height
            self._view_width = widget_height * field_w_per_h
            self._do_rotate_view = False

        elif widget_w_per_h <= field_h_per_w:
            # Widgetが縦長のとき
            self._view_height = widget_width
            self._view_width = widget_width * field_w_per_h
            self._do_rotate_view = True

        else:
            # 描画回転にヒステリシスをもたせる
            if self._do_rotate_view is True:
                self._view_height = widget_height * field_h_per_w
                self._view_width = widget_height
            else:
                self._view_height = widget_width * field_h_per_w
                self._view_width = widget_width

        self._scale_field_to_view = self._view_width / field_width

    
    def _convert_to_view(self, x, y):
        # フィールド座標系を描画座標系に変換する
        view_x = x * self._scale_field_to_view
        view_y = -y * self._scale_field_to_view
        point = QPointF(view_x, view_y)
        return point

    
    def _convert_to_field(self, x, y):
        # 描画座標系をフィールド座標系に変換する
        x /= self._scale.x()
        y /= self._scale.y()

        x -= (self._trans.x() + self._mouse_trans.x())
        y -= (self._trans.y() + self._mouse_trans.y())

        x -= self.width() * 0.5 / self._scale.x()
        y -= self.height() * 0.5 / self._scale.y()

        if self._do_rotate_view:
            x, y = -y, x

        field_x = x / self._scale_field_to_view
        field_y = -y / self._scale_field_to_view
        point = QPointF(field_x, field_y)

        return point

    
    def _reset_painter_status(self):
        # 描画領域の移動と拡大縮小を初期化する
        self._trans = QPointF(0.0, 0.0)
        self._mouse_trans = QPointF(0.0, 0.0)
        self._scale = QPointF(1.0, 1.0)


    def _is_replacement_click(self, mouse_pos):
        # クリックした描画位置にオブジェクトがあればReplacementと判定する
        # ボールとロボットが近い場合、ボールのReplacementを優先する
        field_point = self._convert_to_field(mouse_pos.x(), mouse_pos.y())

        is_clicked = True

        result = self._is_ball_clicked(field_point)
        if result == 'pos':
            self._replacement_target['ball_pos'] = True
            self._replace_func = self._replace_ball_pos
        elif result == 'vel_angle':
            self._replacement_target['ball_vel'] = True
            self._replace_func = self._replace_ball_vel
        else:
            result, robot_id, is_yellow = self._is_robot_clicked(field_point)
            self._replace_id = robot_id
            self._replace_is_yellow = is_yellow

            if result == 'pos':
                self._replacement_target['robot_pos'] = True
                self._replace_func = self._replace_robot_pos
            elif result == 'vel_angle':
                self._replacement_target['robot_angle'] = True
                self._replace_func = self._replace_robot_angle
            else:
                is_clicked = False

        return is_clicked


    def _is_clicked(self, field_point1, field_point2):
        # フィールド上のオブジェクトをクリックしたかどうか判定する
        diff_point = field_point1 - field_point2
        diff_norm = math.hypot(diff_point.x(), diff_point.y())
        if diff_norm < self._REPLACE_CLICK_POS_THRESHOLD:
            return 'pos'
        elif diff_norm < self._REPLACE_CLICK_VEL_ANGLE_THRESHOLD:
            return 'vel_angle'

        return False


    def _is_ball_clicked(self, field_point):
        # ボールをクリックしたか判定する
        # ボールが消えていれば判定しない

        if self._ball_info.disappeared:
            return False

        pos_x = self._ball_info.pose.x
        pos_y = self._ball_info.pose.y
        ball_pos = QPointF(pos_x, pos_y)

        return self._is_clicked(field_point, ball_pos)

    def _is_robot_clicked(self, field_point):
        # ロボットをクリックしたか判定する
        # 消えたロボットは対照外

        is_clicked = False
        replace_id = 0
        is_yellow = False

        for robot in self._robot_info['blue']:
            if robot.disappeared:
                continue
            robot_point = QPointF(robot.pose.x, robot.pose.y)
            is_clicked = self._is_clicked(field_point, robot_point)

            if is_clicked:
                is_yellow = False
                return is_clicked, robot.robot_id, is_yellow

        for robot in self._robot_info['yellow']:
            if robot.disappeared:
                continue
            robot_point = QPointF(robot.pose.x, robot.pose.y)
            is_clicked = self._is_clicked(field_point, robot_point)

            if is_clicked:
                is_yellow = True
                return is_clicked, robot.robot_id, is_yellow

        return is_clicked, replace_id, is_yellow


    def _replace_ball_pos(self, mouse_pos):
        # ボール位置のReplacement
        field_point = self._convert_to_field(mouse_pos.x(), mouse_pos.y())

        ball = ReplaceBall()
        ball.x = field_point.x()
        ball.y = field_point.y()
        ball.is_enabled = True

        if self._invert_side:
            ball.x *= -1.0
            ball.y *= -1.0

        replacements = Replacements()
        replacements.ball = ball
        self._pub_replace.publish(replacements)
        self._replacement_target['ball_pos'] = False


    def _replace_ball_vel(self, mouse_pos):
        # ボール速度のReplacement
        ball_point = QPointF(self._ball_info.pose.x, self._ball_info.pose.y)
        field_point = self._convert_to_field(mouse_pos.x(), mouse_pos.y())
        velocity = self._replacement_velocity(ball_point, field_point)

        ball = ReplaceBall()
        ball.x = ball_point.x()
        ball.y = ball_point.y()
        ball.vx = velocity.x()
        ball.vy = velocity.y()
        ball.is_enabled = True

        if self._invert_side:
            ball.x *= -1.0
            ball.y *= -1.0
            ball.vx *= -1.0
            ball.vy *= -1.0

        replacements = Replacements()
        replacements.ball = ball
        self._pub_replace.publish(replacements)
        self._replacement_target['ball_vel'] = False


    def _replacement_velocity(self, from_point, to_point):
        # Replacementのボール速度を計算する
        diff_point = to_point - from_point
        diff_norm = math.hypot(diff_point.x(), diff_point.y())

        velocity_norm = diff_norm * self._REPLACE_BALL_VELOCITY_GAIN
        if velocity_norm > self._REPLACE_MAX_BALL_VELOCITY:
            velocity_norm = self._REPLACE_MAX_BALL_VELOCITY

        angle = math.atan2(diff_point.y(), diff_point.x())
        velocity = QPointF(
                velocity_norm * math.cos(angle),
                velocity_norm * math.sin(angle))

        return velocity


    def _replace_robot_pos(self, mouse_pos):
        # ロボット位置のReplacement
        field_point = self._convert_to_field(mouse_pos.x(), mouse_pos.y())

        # ロボット角度をradiansからdegreesに変換する
        direction = 0
        if self._replace_is_yellow:
            direction = math.degrees(self._robot_info['yellow'][self._replace_id].pose.theta)
        else:
            direction = math.degrees(self._robot_info['blue'][self._replace_id].pose.theta)

        robot = ReplaceRobot()
        robot.x = field_point.x()
        robot.y = field_point.y()
        robot.dir = direction
        robot.id = self._replace_id
        robot.yellowteam = self._replace_is_yellow
        robot.turnon = True

        if self._invert_side:
            robot.x *= -1.0
            robot.y *= -1.0
            robot.dir += 180

        replacements = Replacements()
        replacements.robots.append(robot)
        self._pub_replace.publish(replacements)
        self._replacement_target['robot_pos'] = False


    def _replace_robot_angle(self, mouse_pos):
        # ロボット角度のReplacement
        field_point = self._convert_to_field(mouse_pos.x(), mouse_pos.y())

        # ロボット角度をradiansからdegreesに変換する
        robot_point = QPointF()
        if self._replace_is_yellow:
            robot_point = QPointF(
                    self._robot_info['yellow'][self._replace_id].pose.x,
                    self._robot_info['yellow'][self._replace_id].pose.y)
        else:
            robot_point = QPointF(
                    self._robot_info['blue'][self._replace_id].pose.x,
                    self._robot_info['blue'][self._replace_id].pose.y)

        robot = ReplaceRobot()
        robot.x = robot_point.x()
        robot.y = robot_point.y()
        robot.dir = math.degrees(self._to_angle(robot_point, field_point))
        robot.id = self._replace_id
        robot.yellowteam = self._replace_is_yellow
        robot.turnon = True

        if self._invert_side:
            robot.x *= -1.0
            robot.y *= -1.0
            robot.dir += 180

        replacements = Replacements()
        replacements.robots.append(robot)
        self._pub_replace.publish(replacements)
        self._replacement_target['robot_angle'] = False


    def _draw_field(self, painter):
        # フィールドの緑と白線を描画する

        # グリーンカーペットを描画
        painter.setPen(Qt.black)
        painter.setBrush(Qt.green)

        rect = QRectF(-self._view_width*0.5, -self._view_height*0.5, 
                self._view_width, self._view_height)
        painter.drawRect(rect)

        # 白線を描画
        painter.setPen(QPen(Qt.white, self._WHITE_LINE_THICKNESS))

        for line in self._field_lines:
            p1 = self._convert_to_view(line["p1_x"], line["p1_y"])
            p2 = self._convert_to_view(line["p2_x"], line["p2_y"])
            painter.drawLine(p1, p2)

        for arc in self._field_arcs:
            top_left = self._convert_to_view(
                    arc["center_x"] - arc["radius"],
                    arc["center_y"] + arc["radius"] )
            size = arc["radius"] * 2.0 * self._scale_field_to_view

            # angle must be 1/16 degrees order
            start_angle = math.degrees(arc["a1"]) * 16
            end_angle = math.degrees(arc["a2"]) * 16
            span_angle = end_angle - start_angle
            painter.drawArc(top_left.x(), top_left.y(), size, size, start_angle, span_angle)

    
    def _draw_ball(self, painter):
        # ボールを描画する

        if self._ball_info.disappeared is False:
            point = self._convert_to_view(
                    self._ball_info.pose.x, self._ball_info.pose.y)
            size = self._BALL_RADIUS * self._scale_field_to_view

            ball_color = copy.deepcopy(self._COLOR_BALL)
            if self._ball_info.detected is False:
                # ボールを検出してないときは透明度を変える
                ball_color.setAlpha(self._ALPHA_NOT_DETECTED)
            painter.setPen(Qt.black)
            painter.setBrush(ball_color)
            painter.drawEllipse(point, size, size)

    
    def _draw_ball_velocity(self, painter):
        # ボールの速度方向を描画する
        VELOCITY_THRESH = 1.0
        PAINT_DIST = 10.0 # meters

        ball_pos = self._ball_info.pose
        ball_vel = self._ball_info.velocity

        # 速度が小さければ描画しない
        if math.hypot(ball_vel.x, ball_vel.y) < VELOCITY_THRESH:
            return

        direction = math.atan2(ball_vel.y, ball_vel.x)

        vel_pos_x = PAINT_DIST * math.cos(direction) + ball_pos.x
        vel_pos_y = PAINT_DIST * math.sin(direction) + ball_pos.y

        point1 = self._convert_to_view(ball_pos.x, ball_pos.y)
        point2 = self._convert_to_view(vel_pos_x, vel_pos_y)

        painter.setPen(QPen(QColor(102, 0, 255), 2))
        painter.drawLine(point1, point2)



    def _draw_robots(self, painter):
        # 全てのロボットを描画する

        for blue in self._robot_info['blue']:
            self._draw_robot(painter, blue, 'blue')
            
        for yellow in self._robot_info['yellow']:
            self._draw_robot(painter, yellow, 'yellow')


    def _draw_robot(self, painter, robot, color):
        # ロボット1台を描画する

        if robot.disappeared is False:
            point = self._convert_to_view(robot.pose.x, robot.pose.y)
            size = self._ROBOT_RADIUS * self._scale_field_to_view

            robot_color = copy.deepcopy(self._COLOR_ROBOT[color])
            if robot.detected is False:
                # ロボットを検出してないときは透明度を変える
                robot_color.setAlpha(self._ALPHA_NOT_DETECTED)
            painter.setPen(Qt.black)
            painter.setBrush(robot_color)
            painter.drawEllipse(point, size, size)

            # ロボット角度
            line_x = self._ROBOT_RADIUS * math.cos(robot.pose.theta)
            line_y = self._ROBOT_RADIUS * math.sin(robot.pose.theta)
            line_point = point + self._convert_to_view(line_x, line_y)
            painter.drawLine(point, line_point)

            # ロボットID
            text_point = point + self._convert_to_view(self._ID_POS[0], self._ID_POS[1])
            painter.drawText(text_point, str(robot.robot_id))

            # ControlTarget
            self._draw_control_target(painter, color, robot)


    def _draw_cursor_coordinate(self, painter):
        # マウスカーソルのフィールド座標を描画する
        current_pos = self._convert_to_field(
                self._current_mouse_pos.x(), self._current_mouse_pos.y())
        current_point = self._convert_to_view(current_pos.x(), current_pos.y())

        text = "(" + str(round(current_pos.x(),2)) + ", " + str(round(current_pos.y(),2)) + ")"

        painter.setPen(Qt.black)
        painter.drawText(current_point, text)


    def _draw_pos_replacement(self, painter):
        # 位置のReplacementを描画する
        start_pos = self._convert_to_field(
                self._click_point.x(), self._click_point.y())
        current_pos = self._convert_to_field(
                self._current_mouse_pos.x(), self._current_mouse_pos.y())

        start_point = self._convert_to_view(start_pos.x(), start_pos.y())
        current_point = self._convert_to_view(current_pos.x(), current_pos.y())

        painter.setPen(QPen(Qt.red, 2))
        painter.drawLine(start_point, current_point)


    def _draw_vel_replacement(self, painter):
        # ボール速度のReplacementを描画する

        current_pos = self._convert_to_field(
                self._current_mouse_pos.x(), self._current_mouse_pos.y())
        current_point = self._convert_to_view(current_pos.x(), current_pos.y())
        ball_pos = QPointF(
                self._ball_info.pose.x, self._ball_info.pose.y)
        ball_point = self._convert_to_view(ball_pos.x(), ball_pos.y())

        painter.setPen(QPen(Qt.blue, 2))
        painter.drawLine(ball_point, current_point)

        # 速度の数値を描画する
        velocity = self._replacement_velocity(ball_pos, current_pos)
        text = "[" + str(round(velocity.x(),2)) + ", " + str(round(velocity.y(),2)) + "]"
        painter.setPen(Qt.black)
        painter.drawText(current_point, text)


    def _draw_angle_replacement(self, painter):
        # ロボット角度のReplacementを描画する
        robot_pos = QPointF()
        if self._replace_is_yellow:
            robot_pos.setX(self._robot_info['yellow'][self._replace_id].pose.x)
            robot_pos.setY(self._robot_info['yellow'][self._replace_id].pose.y)
        else:
            robot_pos.setX(self._robot_info['blue'][self._replace_id].pose.x)
            robot_pos.setY(self._robot_info['blue'][self._replace_id].pose.y)
        robot_point = self._convert_to_view(robot_pos.x(), robot_pos.y())

        current_pos = self._convert_to_field(
                self._current_mouse_pos.x(), self._current_mouse_pos.y())
        current_point = self._convert_to_view(current_pos.x(), current_pos.y())

        painter.setPen(QPen(Qt.blue, 2))
        painter.drawLine(robot_point, current_point)

        # 角度の数値を描画する
        angle = math.degrees(self._to_angle(robot_pos, current_pos))
        text = "[" + str(round(angle,2)) + "]"
        painter.setPen(Qt.black)
        painter.drawText(current_point, text)


    def _draw_control_target(self, painter, color, robot):
        # ロボットの制御目標値を描画する

        # 経路の線を描画するための変数
        prev_point = None

        robot_id = robot.robot_id
        target = self._control_targets[color][robot_id]

        # 経路を描画
        for pose in target.path:
            point = self._convert_to_view(pose.x, pose.y)
            size = self._ROBOT_RADIUS * self._scale_field_to_view

            target_color = QColor(Qt.magenta)
            target_color.setAlphaF(0.5)
            painter.setPen(Qt.black)
            painter.setBrush(target_color)
            painter.drawEllipse(point, size, size)

            # 角度
            line_x = self._ROBOT_RADIUS * math.cos(pose.theta)
            line_y = self._ROBOT_RADIUS * math.sin(pose.theta)
            line_point = point + self._convert_to_view(line_x, line_y)
            painter.drawLine(point, line_point)

            # IDを添える
            text_point = point + self._convert_to_view(self._ID_POS[0], self._ID_POS[1])
            painter.drawText(text_point, str(robot_id))

            # 経路を描画
            if prev_point is None:
                prev_point = point
            else:
                painter.setPen(QPen(QColor(0,0,255, 127), 4))
                painter.drawLine(prev_point, point)
                prev_point = point

        # キック・ドリブルフラグを、ロボットの現在位置周辺に描画
        text_flag = ""
        if target.kick_power > 0.0:
            text_flag += "K:" + str(target.kick_power)

        if target.chip_enable is True:
            text_flag += "\nC:ON"

        if target.dribble_power > 0.0:
            text_flag += "\nD:" + str(target.dribble_power)
    
        point = self._convert_to_view(robot.pose.x, robot.pose.y)
        text_point = point + self._convert_to_view(self._FLAG_POS[0], self._FLAG_POS[1])
        # 複数行に分けてテキストを描画するため、widthとheightを設定する
        text_width = 50
        text_height = 100
        painter.setPen(QPen(Qt.red, 2))
        painter.drawText(
                text_point.x(), text_point.y(), 
                text_width, text_height, 0, text_flag)

    
    def _to_angle(self, from_point, to_point):
        diff_point = to_point - from_point

        return math.atan2(diff_point.y(), diff_point.x())


    def _draw_joy_target(self, painter):
        # JoyStickのControlTargetを描画する

        # 経路の線を描画するための変数
        prev_point = None

        for i, pose in enumerate(self._joy_target.path):
            point = self._convert_to_view(pose.x, pose.y)
            size = self._ROBOT_RADIUS * self._scale_field_to_view

            joy_color = QColor(Qt.magenta)
            painter.setPen(Qt.black)
            painter.setBrush(joy_color)
            painter.drawEllipse(point, size, size)

            # 角度
            line_x = self._ROBOT_RADIUS * math.cos(pose.theta)
            line_y = self._ROBOT_RADIUS * math.sin(pose.theta)
            line_point = point + self._convert_to_view(line_x, line_y)
            painter.drawLine(point, line_point)

            # インデックス
            text_point = point + self._convert_to_view(self._ID_POS[0], self._ID_POS[1])
            painter.drawText(text_point, str(i))

            # 経路を描画
            if prev_point is None:
                prev_point = point
            else:
                painter.setPen(QPen(QColor(0,0,255, 127), 4))
                painter.drawLine(prev_point, point)
                prev_point = point

    def _draw_referee(self, painter):
        # レフェリーの情報を描画する
        PLACE_RADIUS = 0.15 # meters
        AVOID_LENGTH = 0.5 # meter


        if self._decoded_referee is None:
            return 

        ball_pose = self._ball_info.pose

        # ボールプレースメントの進入禁止エリアと設置位置を描画
        if self._decoded_referee.referee_text == "OUR_BALL_PLACEMENT" \
                or self._decoded_referee.referee_text == "THEIR_BALL_PLACEMENT":
            replacement_pose = self._decoded_referee.placement_position

            # 進入禁止エリアを描画
            # Reference: Rule 8.2.3
            angle_ball_to_target = tool.get_angle(ball_pose, replacement_pose)
            dist_ball_to_target = tool.distance_2_poses(ball_pose, replacement_pose)
            trans_BtoT = tool.Trans(ball_pose, angle_ball_to_target)

            # 進入禁止エリア長方形の角の座標を取得
            avoid_upper_left = trans_BtoT.inverted_transform(Pose2D(0, AVOID_LENGTH, 0))
            avoid_lower_left = trans_BtoT.inverted_transform(Pose2D(0, -AVOID_LENGTH, 0))
            avoid_upper_right = trans_BtoT.inverted_transform(
                    Pose2D(dist_ball_to_target, AVOID_LENGTH, 0))
            avoid_lower_right = trans_BtoT.inverted_transform(
                    Pose2D(dist_ball_to_target, -AVOID_LENGTH, 0))

            # 各座標を描画座標に変換
            upper_left_point = self._convert_to_view(
                    avoid_upper_left.x, avoid_upper_left.y)
            lower_left_point = self._convert_to_view(
                    avoid_lower_left.x, avoid_lower_left.y)
            upper_right_point = self._convert_to_view(
                    avoid_upper_right.x, avoid_upper_right.y)
            lower_right_point = self._convert_to_view(
                    avoid_lower_right.x, avoid_lower_right.y)
            # ポリゴンに追加
            polygon = QPolygonF()
            polygon.append(upper_left_point)
            polygon.append(upper_right_point)
            polygon.append(lower_right_point)
            polygon.append(lower_left_point)

            avoid_color = QColor(Qt.red)
            avoid_color.setAlphaF(0.3)
            painter.setPen(QPen(Qt.black,1))
            painter.setBrush(avoid_color)
            painter.drawPolygon(polygon)

            replace_point = self._convert_to_view(
                    replacement_pose.x, replacement_pose.y)
            ball_point = self._convert_to_view(
                    ball_pose.x, ball_pose.y)

            size = AVOID_LENGTH * self._scale_field_to_view
            painter.drawEllipse(replace_point, size, size)
            painter.drawEllipse(ball_point, size, size)


            # ボール設置位置を描画
            size = PLACE_RADIUS * self._scale_field_to_view
            place_color = QColor(Qt.white)
            place_color.setAlphaF(0.6)
            painter.setPen(QPen(Qt.black,2))
            painter.setBrush(place_color)
            painter.drawEllipse(replace_point, size, size)


        # ボール進入禁止エリアを描画
        if self._decoded_referee.keep_out_radius_from_ball != -1:
            point = self._convert_to_view(
                    ball_pose.x, ball_pose.y)
            size = self._decoded_referee.keep_out_radius_from_ball * self._scale_field_to_view

            ball_color = copy.deepcopy(self._COLOR_BALL)
            keepout_color = QColor(Qt.red)
            keepout_color.setAlphaF(0.3)
            painter.setPen(Qt.black)
            painter.setBrush(keepout_color)
            painter.drawEllipse(point, size, size)

        # レフェリーテキストをカーソル周辺に表示する
        if self._decoded_referee.referee_text:
            # カーソル座標を取得
            current_pos = self._convert_to_field(
                    self._current_mouse_pos.x(), self._current_mouse_pos.y())
            # 他のテキストと被らないように位置を微調整
            current_point = self._convert_to_view(current_pos.x() + 0.1, current_pos.y() - 0.15)

            text = self._decoded_referee.referee_text

            painter.setPen(Qt.red)
            painter.drawText(current_point, text)


