import gym
from gym import error, spaces, utils
import json
import numpy as np
# from gym.envs.classic_control import rendering
import math 
import time
import os

NOISE = True

least_v = 0.000001
np.random.seed(626)

class GridChaosEnv(gym.Env):
    """
    Description:
        A triangle is struggling to find her partner because she wants to be square and not sharp. The triangle is born in the left bottom of the grid world, and the partner is in the right top corner. 
        There are two doors, one for her goal, and another one for CHAOS, which is a chaos world where the interaction with environment becomes uncontrolable and unpredictable. The CHAOS world has a wonderful Chinese name "hundun", describing the fuzzy state of the universe before the dawn of time.

    Source:
        This environment corresponds to paper OVD-Explorer: A General Information-theoretic Exploration Approach for Reinforcement Learning.

    Observation:
        Type: Box(2)
        Is: normalized coordinates of the triangle.
        Num     Observation               Min                     Max
        0       x                        -1                         1
        1       y                        -1                         1
        Using the center point of the entire grid world (which is where the two doors meet) as the origin, create an XY-coordinate system. Normalize the coordinates of all the items to (-1, 1). The coordinates of the triangle is the coordinates of its geometric center point, and by default it is (-0.5, -0.5).

    Actions:
        Type: Box(2)
        Num     Name        Min     Max     Action
        0       Degree      -1      1       Controls the forward direction in degrees, and the range (-1, 1) corresponds to [-pi, 0] U [0, pi], 0 means the positive right side. 
        1       Distance    -1      1       Controls the distance moved. -1 means remain still, 0 and 1 mean moving forward by 1 or 2 move_size, respectively.

        Note: Positive angles result from counterclockwise rotation, and negative angles result from clockwise rotation. Illegal movement results in motionlessness, such as hitting a wall.

    Reward:
        Reward is 1 for reaching goal, 0 for others.

    Starting State:
        (-0.5, -0.5)
        # All observations are assigned a uniform random value in [-0.05..0.05]

    Episode Termination:
        Reaching goal, where the coordinates of the triangle is (1, 1)
    
    (It is worth noting that the game is set up in such a way that being adsorbed does not mean that the coefficient of friction is infinite, as movement in the direction of the wall is allowed.)
    """

    metadata = {'render.modes': ['human']}

    def __init__(self):
        self.config  = json.load(open(os.path.join(os.path.dirname(__file__), "env_config.json")))
        self.ms = self.config["move_size"]
        self.viewer = None
        # self.viewer = rendering.Viewer(int(self.config["figure_size"]) + 100, int(self.config["figure_size"]) + 100)
        # self.baserender(self.config)
        self.done = False
        self.lastone = []
        # self.start = np.asarray([-0.9, -0.2])
        self.action_space = spaces.Box(
            low=-1,
            high=1,
            shape=(2,),
            dtype=np.float32
        )
        self.observation_space = spaces.Box(
            low=-1,
            high=1,
            dtype=np.float32,
            shape=(2,),
        )
    
    def baserender(self, config, render=False):
        self.l = 50
        self.ts = self.config["goal_size"]
        self.tsh = self.ts / 2
        self.fs = self.config["figure_size"] 
        self.cgs = self.config["chaos_gate_size"]
        self.ggs = self.config["goal_gate_size"]
        self.cs = self.fs - self.ts
        self.h = self.fs + self.l
        self.m = (self.l+self.h)/2
        self.tcc = (self.l + self.tsh + self.cs / 20, self.m - 2 * self.cs / 20)   # -0.9， -0.2
        # self.tcc = (self.l + self.fs / 4, self.l + 3 * self.fs / 4)   # -0.5， 0.5
        self.trans = (0, 0)
        if render:
            line1 = rendering.Line((self.l, self.l), (self.l, self.fs+self.l))
            line2 = rendering.Line((self.l, self.l), (self.h, self.l))
            line3 = rendering.Line((self.h, self.l), (self.h, self.h))
            line4 = rendering.Line((self.l, self.h), (self.h, self.h))
            line5 = rendering.Line((self.l, self.m), (self.m - self.cgs, self.m))
            line6 = rendering.Line((self.m + self.ggs, self.m), (self.h, self.m))
            line7 = rendering.Line((self.m, self.m), (self.m, self.h))
            # 给元素添加颜色
            line1.set_color(0, 0, 0)
            line2.set_color(0, 0, 0)
            line3.set_color(0, 0, 0)
            line4.set_color(0, 0, 0)
            line5.set_color(0, 0, 0)
            line6.set_color(0, 0, 0)
            line7.set_color(0, 0, 0)
            # 把图形元素添加到画板中

            self.chaos = rendering.make_polygon([(self.l, self.m), (self.l, self.h), (self.m, self.h), (self.m, self.m)], filled=True)
            trs=150
            self.triangle = rendering.make_polygon([(trs, trs), (trs, trs+self.ts), (trs+self.ts, trs+self.ts/2), (trs, trs)], filled=True)
            # self.triangle = rendering.make_polygon([(self.l, self.l), (self.l, self.l+self.ts), (self.l+self.ts, self.l+int(self.ts/2)), (self.l, self.l)], filled=True)
            self.goal_h1 = rendering.make_polygon([(self.h, self.h), (self.h, self.h-self.ts/2), (self.h-self.ts, self.h), (self.h, self.h)], filled=True)
            self.goal_h2 = rendering.make_polygon([(self.h, self.h-self.ts), (self.h-self.ts, self.h-self.ts), (self.h, self.h-self.ts/2), (self.h, self.h-self.ts)], filled=True)
            self.chaos.set_color(0.9, 0.9, 0.9)
            self.triangle.set_color(1, 0, 0)
            self.goal_h1.set_color(0, 0, 1)
            self.goal_h2.set_color(0, 0, 1)
            self.viewer.add_geom(self.chaos)
            # self.viewer.add_geom(self.triangle)
            self.viewer.add_geom(self.goal_h1)
            self.viewer.add_geom(self.goal_h2)
            self.viewer.add_geom(line1)
            self.viewer.add_geom(line2)
            self.viewer.add_geom(line3)
            self.viewer.add_geom(line4)
            self.viewer.add_geom(line5)
            self.viewer.add_geom(line6)
            self.viewer.add_geom(line7)
            self.transform = rendering.Transform(translation=(0, 0))
            self.triangle.add_attr(self.transform)
            self.viewer.add_geom(self.triangle)

    def reset(self, filename=None, render=False):
        self.render(close=True)
        if filename is not None:
            self.config  = json.load(open(filename))
        if render:
            self.viewer = rendering.Viewer(int(self.config["figure_size"]) + 100, int(self.config["figure_size"]) + 100)
        self.baserender(self.config, render=render)
        self.done = False
        if self.config["random_init_x0"] == -1:
            self.state = self.start
            self.tcc = (self.l + self.tsh + self.cs / 20, self.m - 2 * self.cs / 20)   # -0.9， -0.2
        else:
            x_init = round(np.random.uniform(self.config["random_init_x0"], self.config["random_init_x1"]), 2)
            y_init = round(np.random.uniform(self.config["random_init_y0"], self.config["random_init_y1"]), 2)
            self.tcc = (x_init, y_init)
            self.state = np.asarray(self.ori_coo2one_coo(self.tcc))
            print("Env Init", self.config, self.tcc, self.state)
        # self.tcc = (self.l + self.fs / 4, self.l + 3 * self.fs / 4)
        self.trans = (0, 0)
        self.ooo = self.tcc
        # print(self.ori_coo2one_coo(self.tcc), self.state, self.ori_coo2one_coo(self.tcc)[1] - self.state[1])
        assert self.ori_coo2one_coo(self.tcc)[0] - self.state[0] < 0.1 and self.ori_coo2one_coo(self.tcc)[0] - self.state[0] > -0.1
        assert self.ori_coo2one_coo(self.tcc)[1] - self.state[1] < 0.1 and self.ori_coo2one_coo(self.tcc)[1] - self.state[1] > -0.1
        self.lastone.append(self.tcc)
        # assert self.ori_coo2one_coo(self.tcc)[1] == self.state[1]
        # self.viewer = rendering.Viewer(self.config["figure_size"] + 100, self.config["figure_size"] + 100)
        return self.state

    def step(self, action):
        assert self.action_space.contains(action)

        self.state = self.step_(action)
        self.done = self.state[0] > 0.95 and self.state[1] > 0.95
        reward = self.get_reward()
        return self.state, reward, self.done, None
    
    def get_reward(self):
        reward = self.config["reward_goal"] if self.done else self.config["reward_r"] * (-math.sqrt(math.pow(1-self.state[0], 2) + math.pow(1-self.state[1], 2)) / (math.sqrt(2) * 2))
        return reward
    
    def need_noise(self, x1, y1):
        # return True
        if not NOISE:
            return False
        if y1 >= self.l + self.tsh - least_v and y1 <= self.m - self.tsh + least_v:
            return False
        if y1 <= self.h + self.tsh - least_v and y1 >= self.m - self.tsh - least_v:
            if x1 >= self.l + self.tsh - least_v and x1 <= self.m - self.tsh + least_v:
                return True
            if x1 <= self.h - self.tsh + least_v and x1 >= self.m + self.tsh - least_v:
                return False
    
    def add_noise(self, x1, y1):
        # return True
        if not NOISE:
            return np.random.uniform(0, 0.0)
        if y1 >= self.l + self.tsh - least_v and y1 <= self.m - self.tsh + least_v:
            if x1 >= self.l + self.tsh - least_v and x1 <= self.m:
                return np.random.normal(0, 0.01)
            if x1 <= self.h - self.tsh + least_v and x1 >= self.m:
                return np.random.normal(0, 0.05)
        elif y1 <= self.h - self.tsh + least_v and y1 >= self.m - self.tsh - least_v:
            if x1 >= self.l + self.tsh - least_v and x1 <= self.m - self.tsh + least_v:
                return np.random.normal(0, 0.01)
            if x1 <= self.h - self.tsh + least_v and x1 >= self.m + self.tsh - least_v:
                return np.random.normal(0, 0.05)
        else:
            print("#ENVERROR - NOISE", x1, y1)
            return np.random.uniform(0, 0.0)
        
    def step_(self, action):
        # if self.need_noise(self.tcc[0], self.tcc[1]):
        # np.random.normal(0, np.random.uniform(0.001, 0.01))
        action[0] +=  self.add_noise(self.tcc[0], self.tcc[1])
        action[1] +=  self.add_noise(self.tcc[0], self.tcc[1])
        action[0] = 1 if action[0] > 1 else action[0]
        action[0] = -1 if action[0] < -1 else action[0]
        action[1] = -1 if action[1] < -1 else action[1]
        action[1] = 1 if action[1] > 1 else action[1]
        degree_ = action[0] * math.pi
        distance_ = (action[1] + 1) * self.ms 
        # # # print("degree_, distance_", degree_, distance_)
        orinx = distance_ * math.cos(degree_)
        oriny = distance_ * math.sin(degree_)
        # # # print("orinx, oriny", orinx, oriny)
        o = self.tcc
        # print("\tn", self.tcc, action, self.tcc[0]+orinx, self.tcc[1]+oriny)
        self.tcc = self.judge_hit(self.tcc[0], self.tcc[1], round(self.tcc[0]+orinx, 2), round(self.tcc[1]+oriny, 2), degree_)
        self.lastone.append(self.tcc)
        self.lastone = self.lastone[-10:]
        # # # print("self.tcc, o", self.tcc, o, self.ooo)
        self.trans = (self.tcc[0] - self.ooo[0], self.tcc[1] - self.ooo[1])
        # # # print("self.trans", self.trans)
        return np.asarray(self.ori_coo2one_coo(self.tcc))
    
    def ori_coo2one_coo(self, coo):
        ori_coo = (coo[0] - self.m, coo[1] - self.m)
        # # # # print(coo, self.m, ori_coo, ori_coo[0] * 2 / self.cs)
        return (ori_coo[0] * 2 / self.cs, ori_coo[1] * 2 / self.cs)  

    def judge_hit(self, x1, y1, x2, y2, d):
        class Point(): #定义类
            def __init__(self,x,y):
                self.x=x
                self.y=y   

        def cross(p1,p2,p3):#跨立实验
            x1=p2.x-p1.x
            y1=p2.y-p1.y
            x2=p3.x-p1.x
            y2=p3.y-p1.y
            return x1*y2-x2*y1     

        def calculate_z(a, b, c, d):
            return (d.x-c.x)*(b.x-a.x)+(d.y-c.y)*(b.y-a.y)

        def calculate_m(a, b, c, d):
            return math.sqrt(math.pow((d.x-c.x), 2)+math.pow((d.y-c.y), 2))*math.sqrt(math.pow((b.x-a.x), 2)+math.pow((b.y-a.y), 2))

        def isIntersec(p1,p2,p3,p4): #判断两线段是否相交
            c = Point(self.tsh + self.m - self.cgs, self.m - self.tsh)
            d = Point(self.tsh + self.m - self.cgs, self.m + self.tsh)
            h = Point(self.m - self.tsh, self.m - self.tsh)
            i = Point(self.m + self.tsh, self.m - self.tsh)
            m = Point(self.m - self.tsh + self.ggs, self.m + self.tsh)
            n = Point(self.m - self.tsh + self.ggs, self.m - self.tsh)
            # # # # print("a")
            x_, y_ = -1, -1
            #快速排斥，以l1、l2为对角线的矩形必相交，否则两线段不相交
            # if True:
            if (max(p1.x,p2.x)>=min(p3.x,p4.x)    #矩形1最右端大于矩形2最左端
            and max(p3.x,p4.x)>=min(p1.x,p2.x)   #矩形2最右端大于矩形最左端
            and max(p1.y,p2.y)>=min(p3.y,p4.y)   #矩形1最高端大于矩形最低端
            and max(p3.y,p4.y)>=min(p1.y,p2.y)): #矩形2最高端大于矩形最低端
            #若通过快速排斥则进行跨立实验
                # # print(cross(p1,p2,p3)*cross(p1,p2,p4))
                # # print(cross(p3,p4,p1)*cross(p3,p4,p2))
                if(cross(p1,p2,p3)*cross(p1,p2,p4)<=0 and cross(p3,p4,p1)*cross(p3,p4,p2)<=0):
                    # # print( (   (p1.y - p2.y)*(p3.x - p4.x) - (p3.y - p4.y)*(p1.x - p2.x) ))
                    if (   (p1.y - p2.y)*(p3.x - p4.x) - (p3.y - p4.y)*(p1.x - p2.x) )!= 0:
                        x_ = ( (p3.x-p4.x) * (  (p1.y - p2.y)*p1.x - (p1.x - p2.x)*p1.y  ) - (p1.x - p2.x) * ( (p3.y - p4.y)*p3.x - (p3.x - p4.x)*p3.y ) ) / (   (p1.y - p2.y)*(p3.x - p4.x) - (p3.y - p4.y)*(p1.x - p2.x)  )
                        y_ = ( (p3.y-p4.y) * (  (p1.y - p2.y)*p1.x - (p1.x - p2.x)*p1.y  ) - (p1.y - p2.y) * ( (p3.y - p4.y)*p3.x - (p3.x - p4.x)*p3.y ) ) / (   (p1.y - p2.y)*(p3.x - p4.x) - (p3.y - p4.y)*(p1.x - p2.x)  )
                        x_ = round(x_, 2)
                        y_ = round(y_, 2)
                        # if (x_==c.x and y_==c.y and calculate_z(p1, p2, c, d)/calculate_m(p1, p2, c, d) > 0) or (x_==d.x and y_==d.y and (calculate_z(p1, p2, d, c)/calculate_m(p1, p2, d, c)) > 0) or x_==h.x and y_==h.y and (calculate_z(p1, p2, h, i)/calculate_m(p1, p2, h, i)) > 0 or x_==i.x and y_==i.y and (calculate_z(p1, p2, i, h)/calculate_m(p1, p2, i, h)) > 0 or x_==m.x and y_==m.y and (calculate_z(p1, p2, m, n)/calculate_m(p1, p2, m, n)) > 0 or x_==n.x and y_==n.y and (calculate_z(p1, p2, n, m)/calculate_m(p1, p2, n, m)) > 0:
                        #     D=-1
                        #     print(D)
                        # else: 
                        #     D=1 # 相交
                        D=1 # 相交
                    else:
                        D=-1   # 相切
                else:
                    D=0 #没有影响
            else:
                D=0
            return D, x_, y_

        def which_area(a: Point):
            area = []
            if a.y >= self.l + self.tsh - least_v and a.y <= self.m - self.tsh + least_v:
                area.append(0)
            if a.y <= self.h - self.tsh + least_v and a.y >= self.m + self.tsh - least_v:
                if a.x >= self.l + self.tsh - least_v and a.x <= self.m - self.tsh + least_v:
                    area.append(1)
                if a.x <= self.h - self.tsh + least_v and a.x >= self.m + self.tsh - least_v:
                    # # # print("-", a.x,self.h - self.tsh + least_v , a.x, self.m + self.tsh - least_v)
                    area.append(2)
            elif a.y < self.m + self.tsh and a.y >= self.m - self.tsh - least_v:
                if a.x >= self.tsh + self.m - self.cgs - least_v and a.x <= self.m - self.tsh + least_v:
                    area.append(1)
                if a.x <= self.m - self.tsh + self.ggs + least_v and a.x >= self.m + self.tsh - least_v:
                    # # # print("-", a.x,self.h - self.tsh + least_v , a.x, self.m + self.tsh - least_v)
                    area.append(2)
            assert len(area) != 0
            return area
        
        def bordersofarea(area: int, d_):
            a = Point(self.l + self.tsh, self.l + self.tsh)
            b = Point(self.l + self.tsh, self.m - self.tsh)
            c = Point(self.tsh + self.m - self.cgs, self.m - self.tsh)
            d = Point(self.tsh + self.m - self.cgs, self.m + self.tsh)
            e = Point(self.l + self.tsh, self.m + self.tsh)
            f = Point(self.l + self.tsh, self.h - self.tsh)
            g = Point(self.m - self.tsh, self.h - self.tsh)
            h = Point(self.m - self.tsh, self.m - self.tsh)
            i = Point(self.m + self.tsh, self.m - self.tsh)
            j = Point(self.m + self.tsh, self.h - self.tsh)
            k = Point(self.h - self.tsh, self.h - self.tsh)
            l = Point(self.h - self.tsh, self.m + self.tsh)
            m = Point(self.m - self.tsh + self.ggs, self.m + self.tsh)
            n = Point(self.m - self.tsh + self.ggs, self.m - self.tsh)
            o = Point(self.h - self.tsh, self.m - self.tsh)
            p = Point(self.h - self.tsh, self.l + self.tsh)
            # if d == 0:
            #     return []
            # else:
            # 原则:不要有平行的线同时评估,尤其是距离小的
            # 锐角钝角分开
            if area == 0:   
                if d_ == 0:
                    return [(o, p)]
                elif d_ == math.pi or d_ == - math.pi:
                    return [(a, b)]
                elif d_ < 0 :
                    if d_ > - math.pi / 2:       
                        return [(a, p), (o, p)]
                    elif d_ < - math.pi / 2:       
                        return [(a, b), (a, p)]
                    elif d_ == - math.pi / 2:       
                        return [(a, p)]
                    else:
                        assert False == True
                    # elif d_ < 0:       
                    #     return [(a, b), (a, p), (o, p)]
                else:
                    if d_ < math.pi / 2:
                        return [(b, c), (g, h), (h, i), (m, n), (n, o), (o, p), (k, l)]
                    elif d_ > math.pi / 2:
                        return [(n, o), (j, i), (h, i), (c, d), (b, c), (a, b), (e, f)]
                    else:
                        return [(b, c), (n, o), (h, i)]
            elif area == 1:
                if d_ == 0 :
                    return [(g, h)]
                elif d_ == math.pi or d_ == - math.pi:
                    return [(c, d), (e, f)]
                elif d_ > 0:
                    if d_ < math.pi / 2:
                        return [(g, h), (f, g)]
                    elif d_ > math.pi / 2:
                        return [(c, d), (e, f), (f, g)]
                    else:
                        return [(f, g)]
                else:
                    if -d_ < math.pi / 2:
                        return [(d, e), (g, h)]
                    elif -d_ > math.pi / 2:
                        return [(e, f), (d, e), (c, d), (a, b)]
                    else:
                        return [(d, e)]
            elif area == 2:
                if d_ == 0:
                    return [(m, n), (k, l)]
                elif d_ == math.pi or d_ == - math.pi:
                    return [(i, j)]
                elif d_ > 0:
                    if d_ < math.pi / 2:
                        return [(j, k), (m, n), (k, l)]
                    elif d_ > math.pi / 2:
                        return [(i, j), (j, k)]
                    else:
                        return [(j, k)]
                else:
                    if -d_ < math.pi / 2:
                        return [(k, l), (m, n), (m, l), (o, p)]
                    elif -d_ > math.pi / 2:
                        return [(m, l), (i, j)]
                    else:
                        return [(m, l)]
            print("ERROR, area", area, d_)
            assert False == True

        # 移动轨迹线段
        m0, m1 = Point(x1, y1), Point(x2, y2)
        # # print(x1, y1, x2, y2, d)
        # if d == 0:
        #     return (m1.x, m1.y)
        # else:
        for area in which_area(m0):            
            # if NOISE:
            #     if area != 2:
            #         m1.x +=  np.random.normal(0, np.random.uniform(0.05, 0.25))
            #         m1.y +=  np.random.normal(0, np.random.uniform(0.05, 0.25))
            borders = bordersofarea(area, d)
            # # print(m0.x, m0.y, "is in area", area, len(borders), m1.x, m1.y)
            ps = []
            for border_ in borders:
                intersection, in_x, in_y = isIntersec(m0, m1, border_[0], border_[1])
                # print("\t",  m0.x, m0.y, m1.x, m1.y, border_[0].x, border_[0].y, border_[1].x, border_[1].y,"\t", intersection, in_x, in_y)
                if intersection == 0:
                    continue
                elif intersection == -1:
                    continue
                elif intersection == 1:
                    # # print(intersection, (x2, y2), "to", (in_x, in_y))
                    if  abs(border_[0].x - border_[1].x) < least_v:
                        # assert in_y < max(border_[0].y, border_[1].y)
                        if abs(in_y - border_[0].y) < least_v:
                            in_y = border_[0].y
                        elif abs(in_y - border_[1].y) < least_v:
                            in_y = border_[1].y
                        ps.append([border_[0].x, in_y, border_[0], border_[1]])
                    elif abs(border_[0].y - border_[1].y) < least_v:
                        # assert in_x < max(border_[0].x, border_[1].x)
                        if abs(in_x - border_[0].x) < least_v:
                            in_x = border_[0].x
                        elif abs(in_x - border_[1].x) < least_v:
                            in_x = border_[1].x
                        ps.append([in_x, border_[0].y, border_[0], border_[1]])
                    else:
                        assert False == True
                        continue
                else:
                    continue
            if len(ps) == 0:
                pass
            elif len(ps) == 1:
                # print("\t", (ps[0][0], ps[0][1]))
                return (round(ps[0][0], 2), round(ps[0][1], 2))
            elif len(ps) == 2:
                if (ps[0][2].x == ps[1][2].x and ps[0][2].y == ps[1][2].y) or (ps[0][2].x == ps[1][3].x and ps[0][2].y == ps[1][3].y):
                    return (round(ps[0][2].x, 2), round(ps[0][2].y, 2))
                elif (ps[0][3].x == ps[1][2].x and ps[0][3].y == ps[1][2].y) or (ps[0][3].x == ps[1][3].x and ps[0][3].y == ps[1][3].y):
                    return (round(ps[0][3].x, 2), round(ps[0][3].y, 2))
                # # print(ps)
                # if ps[0][2].x == ps[0][3].x:
                #     ps[0][0] = ps[0][3].x
                #     ps[1][0] = ps[0][3].x
                # elif ps[0][2].y == ps[0][3].y:
                #     ps[0][1] = ps[0][3].y
                #     ps[1][1] = ps[0][3].y
                # elif ps[1][2].x == ps[1][3].x:
                #     ps[0][0] = ps[1][3].x
                #     ps[1][0] = ps[1][3].x
                # elif ps[1][2].y == ps[1][3].y:
                #     ps[0][1] = ps[1][3].y
                #     ps[1][1] = ps[1][3].y
                else:
                    # assert False == True
                    return (round(ps[0][0], 2), round(ps[0][1], 2))
            elif len(ps) > 2:
                return (round(ps[0][0], 2), round(ps[0][1], 2))
                # return (ps[0][0], ps[0][1])
            return (m1.x, m1.y)

    def render(self, mode='human', close=False, tra=False, s=False):
        if close:
            if self.viewer is not None:
                self.viewer.close()
                self.viewer = None
            return
        if mode == 'human':
            self.transform.set_translation(*self.trans)
            if tra:
                for i in range(len(self.lastone) - 1):
                    linec0 = rendering.Line((self.lastone[i][0], self.lastone[i][1]), (self.lastone[i+1][0], self.lastone[i+1][1]))
                    linec0.set_color(0.8, 0.5, 0)
                    if s:
                        self.viewer.add_onetime(linec0)
                    else:
                        self.viewer.add_geom(linec0)
            return self.viewer.render(return_rgb_array=mode == 'rgb_array')
        elif mode == "rgb_array":
            return 
        else:
            return      
    
    def close(self):
        if self.viewer:
            self.viewer.close()
            self.viewer = None

    def test(self):
        action = [1.0, -0.046686086]
        self.tcc = [325.0, 275.0]
        # if self.need_noise(self.tcc[0], self.tcc[1]):
        #     action[0] +=  np.random.normal(0, np.random.uniform(0.05, 0.75))
        #     action[1] +=  np.random.normal(0, np.random.uniform(0.05, 0.75))
        #     action[0] = 1 if action[0] > 1 else action[0]
        #     action[0] = -1 if action[0] < -1 else action[0]
        #     action[1] = -1 if action[1] < -1 else action[1]
        #     action[1] = 1 if action[1] > 1 else action[1]
        degree_ = action[0] * math.pi
        distance_ = (action[1] + 1) * self.ms 
        print("degree_, distance_", degree_, distance_, self.tcc, action, action[0]+0.5623295, action[1]-0.4887485)
        orinx = distance_ * math.cos(degree_)
        oriny = distance_ * math.sin(degree_)
        print("orinx, oriny", orinx, oriny)
        print("c", self.tcc, self.ori_coo2one_coo(self.tcc))
        o = self.tcc
        self.tcc = self.judge_hit(self.tcc[0], self.tcc[1], round(self.tcc[0]+orinx, 2), round(self.tcc[1]+oriny, 2), degree_)
        print("c", self.tcc, self.ori_coo2one_coo(self.tcc))

# a = GridChaosEnv()
# a.reset()
# # for _ in range(1):
#    a.test()

