import numpy as np
import matplotlib.pyplot as plt
import math

class TwoWheeledSystem():
    """SampleSystem, this is the simulator
    Attributes
    -----------
    x_1 : float
        system state 1
    x_2 : float
        system state 2
    history_x_1 : list
        time history of system state 1 (x_1)
    history_x_2 : list
        time history of system state 2 (x_2)
    """
    def __init__(self, init_x_1=0., init_x_2=0., init_x_3=0.):
        """
        Palameters
        -----------
        init_x_1 : float, optional
            initial value of x_1, default is 0.
        init_x_2 : float, optional
            initial value of x_2, default is 0.
        init_x_3 : float, optional
            initial value of x_3, default is 0.
        """
        self.x_1 = init_x_1
        self.x_2 = init_x_2
        self.x_3 = init_x_3
        self.history_x_1 = [init_x_1]
        self.history_x_2 = [init_x_2]
        self.history_x_3 = [init_x_3]

    def update_state(self, u_1, u_2, dt=0.01):
        """
        Palameters
        ------------
        u_1 : float
            input of system in some cases this means the reference, u_velocity
        u_2 : float
            input of system in some cases this means the reference, u_omega
        dt : float in seconds, optional
            sampling time of simulation, default is 0.01 [s]
        """
        # for theta 1, theta 1 dot, theta 2, theta 2 dot
        k0 = [0.0 for _ in range(3)]
        k1 = [0.0 for _ in range(3)]
        k2 = [0.0 for _ in range(3)]
        k3 = [0.0 for _ in range(3)]

        functions = [self._func_x_1, self._func_x_2, self._func_x_3]

        # solve Runge-Kutta
        for i, func in enumerate(functions):
            k0[i] = dt * func(self.x_1, self.x_2, self.x_3, u_1, u_2)
        
        for i, func in enumerate(functions):
            k1[i] = dt * func(self.x_1 + k0[0]/2., self.x_2 + k0[1]/2., self.x_3 + k0[2]/2., u_1, u_2)
        
        for i, func in enumerate(functions):
            k2[i] = dt * func(self.x_1 + k0[0]/2., self.x_2 + k0[1]/2., self.x_3 + k0[2]/2., u_1, u_2)
        
        for i, func in enumerate(functions):
            k3[i] =  dt * func(self.x_1 + k2[0], self.x_2 + k2[1], self.x_3 + k2[2], u_1, u_2)
        
        self.x_1 += (k0[0] + 2. * k1[0] + 2. * k2[0] + k3[0]) / 6.
        self.x_2 += (k0[1] + 2. * k1[1] + 2. * k2[1] + k3[1]) / 6.
        self.x_3 += (k0[2] + 2. * k1[2] + 2. * k2[2] + k3[2]) / 6.
    
        # save
        self.history_x_1.append(self.x_1)
        self.history_x_2.append(self.x_2)
        self.history_x_3.append(self.x_3)      

    def _func_x_1(self, y_1, y_2, y_3, u_1, u_2):
        """
        Parameters
        ------------
        y_1 : float
        y_2 : float
        y_3 : float
        u_1 : float
            system input
        u_2 : float
            system input
        """
        y_dot = math.cos(y_3) * u_1
        return y_dot
    
    def _func_x_2(self, y_1, y_2, y_3, u_1, u_2):
        """
        Parameters
        ------------
        y_1 : float
        y_2 : float
        y_3 : float
        u_1 : float
            system input
        u_2 : float
            system input
        """
        y_dot = math.sin(y_3) * u_1
        return y_dot
    
    def _func_x_3(self, y_1, y_2, y_3, u_1, u_2):
        """
        Parameters
        ------------
        y_1 : float
        y_2 : float
        y_3 : float
        u_1 : float
            system input
        u_2 : float
            system input
        """
        y_dot = u_2
        return y_dot

class NMPCSimulatorSystem():
    """SimulatorSystem for nmpc, this is the simulator of nmpc
    the reason why I seperate the real simulator and nmpc's simulator is sometimes the modeling error, disturbance can include in real simulator
    Attributes
    -----------
    None

    """
    def __init__(self):
        """
        Parameters
        -----------
        None
        """
        pass

    def calc_predict_and_adjoint_state(self, x_1, x_2, x_3, u_1s, u_2s, N, dt):
        """main
        Parameters
        ------------
        x_1 : float
            current state
        x_2 : float
            current state
        x_3 : float
            current state
        u_1s : list of float
            estimated optimal input Us for N steps
        u_2s : list of float
            estimated optimal input Us for N steps
        N : int
            predict step
        dt : float
            sampling time

        Returns
        --------
        x_1s : list of float
            predicted x_1s for N steps
        x_2s : list of float
            predicted x_2s for N steps
        x_3s : list of float
            predicted x_3s for N steps
        lam_1s : list of float
            adjoint state of x_1s, lam_1s for N steps
        lam_2s : list of float
            adjoint state of x_2s, lam_2s for N steps
        lam_3s : list of float
            adjoint state of x_3s, lam_3s for N steps
        """
        x_1s, x_2s, x_3s = self._calc_predict_states(x_1, x_2, x_3, u_1s, u_2s, N, dt) # by usin state equation
        lam_1s, lam_2s, lam_3s = self._calc_adjoint_states(x_1s, x_2s, x_3s, u_1s, u_2s, N, dt) # by using adjoint equation

        return x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s

    def _calc_predict_states(self, x_1, x_2, x_3, u_1s, u_2s, N, dt):
        """ 
        Parameters
        ------------
        x_1 : float
            current state
        x_2 : float
            current state
        x_3 : float
            current state
        u_1s : list of float
            estimated optimal input Us for N steps
        u_2s : list of float
            estimated optimal input Us for N steps
        N : int
            predict step
        dt : float
            sampling time
            
        Returns
        --------
        x_1s : list of float
            predicted x_1s for N steps
        x_2s : list of float
            predicted x_2s for N steps
        x_3s : list of float
            predicted x_3s for N steps
        """
        # initial state
        x_1s = [x_1]
        x_2s = [x_2]
        x_3s = [x_3]

        for i in range(N):
            temp_x_1, temp_x_2, temp_x_3 = self._predict_state_with_oylar(x_1s[i], x_2s[i], x_3s[i], u_1s[i], u_2s[i], dt)
            x_1s.append(temp_x_1)
            x_2s.append(temp_x_2)
            x_3s.append(temp_x_3)

        return x_1s, x_2s, x_3s

    def _calc_adjoint_states(self, x_1s, x_2s, x_3s, u_1s, u_2s, N, dt):
        """
        Parameters
        ------------
        x_1s : list of float
            predicted x_1s for N steps
        x_2s : list of float
            predicted x_2s for N steps
        x_3s : list of float
            predicted x_3s for N steps
        u_1s : list of float
            estimated optimal input Us for N steps
        u_2s : list of float
            estimated optimal input Us for N steps
        N : int
            predict step
        dt : float
            sampling time
            
        Returns
        --------
        lam_1s : list of float
            adjoint state of x_1s, lam_1s for N steps
        lam_2s : list of float
            adjoint state of x_2s, lam_2s for N steps
        lam_3s : list of float
            adjoint state of x_2s, lam_2s for N steps
        """
        # final state
        # final_state_func
        lam_1s = [x_1s[-1]]
        lam_2s = [x_2s[-1]]
        lam_3s = [x_3s[-1]]

        for i in range(N-1, 0, -1): 
            temp_lam_1, temp_lam_2, temp_lam_3 = self._adjoint_state_with_oylar(x_1s[i], x_2s[i], x_3s[i], lam_1s[0] ,lam_2s[0], lam_3s[0], u_1s[i], u_2s[i], dt)
            lam_1s.insert(0, temp_lam_1)
            lam_2s.insert(0, temp_lam_2)
            lam_3s.insert(0, temp_lam_3)

        return lam_1s, lam_2s, lam_3s

    def final_state_func(self):
        """this func usually need
        """
        pass

    def _predict_state_with_oylar(self, x_1, x_2, x_3, u_1, u_2, dt):
        """in this case this function is the same as simulator
        Parameters
        ------------
        x_1 : float
            system state
        x_2 : float
            system state
        x_3 : float
            system state
        u_1 : float
            system input
        u_2 : float
            system input
        dt : float in seconds
            sampling time
        Returns
        --------
        next_x_1 : float
            next state, x_1 calculated by using state equation
        next_x_2 : float
            next state, x_2 calculated by using state equation
        next_x_3 : float
            next state, x_3 calculated by using state equation
        """
        k0 = [0. for _ in range(3)]

        functions = [self.func_x_1, self.func_x_2, self.func_x_3]

        for i, func in enumerate(functions):
            k0[i] = dt * func(x_1, x_2, x_3, u_1, u_2)
                
        next_x_1 = x_1 + k0[0]
        next_x_2 = x_2 + k0[1]
        next_x_3 = x_3 + k0[2]

        return next_x_1, next_x_2, next_x_3

    def func_x_1(self, y_1, y_2, y_3, u_1, u_2):
        """
        Parameters
        ------------
        y_1 : float
        y_2 : float
        y_3 : float
        u_1 : float
            system input
        u_2 : float
            system input
        """
        y_dot = math.cos(y_3) * u_1
        return y_dot
    
    def func_x_2(self, y_1, y_2, y_3, u_1, u_2):
        """
        Parameters
        ------------
        y_1 : float
        y_2 : float
        y_3 : float
        u_1 : float
            system input
        u_2 : float
            system input
        """
        y_dot = math.sin(y_3) * u_1
        return y_dot
    
    def func_x_3(self, y_1, y_2, y_3, u_1, u_2):
        """
        Parameters
        ------------
        y_1 : float
        y_2 : float
        y_3 : float
        u_1 : float
            system input
        u_2 : float
            system input
        """
        y_dot = u_2
        return y_dot

    def _adjoint_state_with_oylar(self, x_1, x_2, x_3, lam_1, lam_2, lam_3, u_1, u_2, dt):
        """
        Parameters
        ------------
        x_1 : float
            system state
        x_2 : float
            system state
        x_3 : float
            system state
        lam_1 : float
            adjoint state
        lam_2 : float
            adjoint state
        lam_3 : float
            adjoint state
        u_1 : float
            system input
        u_2 : float
            system input
        dt : float in seconds
            sampling time
        Returns
        --------
        pre_lam_1 : float
            pre, 1 step before lam_1 calculated by using adjoint equation
        pre_lam_2 : float
            pre, 1 step before lam_2 calculated by using adjoint equation
        pre_lam_3 : float
            pre, 1 step before lam_3 calculated by using adjoint equation
        """
        k0 = [0. for _ in range(3)]

        functions = [self._func_lam_1, self._func_lam_2, self._func_lam_3]

        for i, func in enumerate(functions):
            k0[i] = dt * func(x_1, x_2, x_3, lam_1, lam_2, lam_3, u_1, u_2)
                
        pre_lam_1 = lam_1 + k0[0]
        pre_lam_2 = lam_2 + k0[1]
        pre_lam_3 = lam_3 + k0[2]

        return pre_lam_1, pre_lam_2, pre_lam_3

    def _func_lam_1(self, y_1, y_2, y_3, y_4, y_5, y_6, u_1, u_2):
        """calculating -\dot{lam_1}
        Parameters
        ------------
        y_1 : float
            means x_1
        y_2 : float
            means x_2
        y_3 : float
            means x_3
        y_4 : float
            means lam_1
        y_5 : float
            means lam_2
        y_6 : float
            means lam_3
        u_1 : float
            means system input
        u_2 : float
            means system input
        Returns
        ---------
        y_dot : float
            means -\dot{lam_1}
        """
        y_dot = 0.
        return y_dot

    def _func_lam_2(self, y_1, y_2, y_3, y_4, y_5, y_6, u_1, u_2):
        """calculating -\dot{lam_2}
        Parameters
        ------------
        y_1 : float
            means x_1
        y_2 : float
            means x_2
        y_3 : float
            means x_3
        y_4 : float
            means lam_1
        y_5 : float
            means lam_2
        y_6 : float
            means lam_3
        u_1 : float
            means system input
        u_2 : float
            means system input
        Returns
        ---------
        y_dot : float
            means -\dot{lam_2}
        """
        y_dot = 0.
        return y_dot

    def _func_lam_3(self, y_1, y_2, y_3, y_4, y_5, y_6, u_1, u_2):
        """calculating -\dot{lam_3}
        Parameters
        ------------
        y_1 : float
            means x_1
        y_2 : float
            means x_2
        y_3 : float
            means x_3
        y_4 : float
            means lam_1
        y_5 : float
            means lam_2
        y_6 : float
            means lam_3
        u_1 : float
            means system input
        u_2 : float
            means system input
        Returns
        ---------
        y_dot : float
            means -\dot{lam_3}
        """
        y_dot = - y_4 * math.sin(y_3) * u_1 + y_5 * math.cos(y_3) * u_1 
        return y_dot

class NMPCController_with_CGMRES():
    """
    Attributes
    ------------
    zeta : float
        gain of optimal answer stability
    ht : float
        update value of NMPC this should be decided by zeta
    tf : float
        predict time
    alpha : float
        gain of predict time
    N : int
        predicte step, discritize value
    threshold : float
        cgmres's threshold value
    input_num : int
        system input length, this should include dummy u and constraint variables
    max_iteration : int
        decide by the solved matrix size
    simulator : NMPCSimulatorSystem class
    u_1s : list of float
        estimated optimal system input
    u_2s : list of float
        estimated optimal system input
    dummy_u_1s : list of float
        estimated dummy input
    dummy_u_2s : list of float
        estimated dummy input
    raw_1s : list of float
        estimated constraint variable
    raw_2s : list of float
        estimated constraint variable
    history_u_1 : list of float
        time history of actual system input
    history_u_2 : list of float
        time history of actual system input
    history_dummy_u_1 : list of float
        time history of actual dummy u_1
    history_dummy_u_2 : list of float
        time history of actual dummy u_2
    history_raw_1 : list of float
        time history of actual raw_1
    history_raw_2 : list of float
        time history of actual raw_2
    history_f : list of float
        time history of error of optimal
    """
    def __init__(self):
        """
        Parameters
        -----------
        None
        """
        # parameters
        self.zeta = 100. # 安定化ゲイン
        self.ht = 0.01 # 差分近似の幅
        self.tf = 1. # 最終時間
        self.alpha = 0.5 # 時間の上昇ゲイン
        self.N = 10 # 分割数
        self.threshold = 0.001 # break値

        self.input_num = 6 # dummy, 制約条件に対するuにも合わせた入力の数
        self.max_iteration = self.input_num * self.N

        # simulator
        self.simulator = NMPCSimulatorSystem()

        # initial
        self.u_1s = np.ones(self.N) * 1.
        self.u_2s = np.ones(self.N) * 0.1
        self.dummy_u_1s = np.ones(self.N) * 0.1
        self.dummy_u_2s = np.ones(self.N) * 2.5
        self.raw_1s = np.ones(self.N) * 0.8
        self.raw_2s = np.ones(self.N) * 0.8

        # for fig
        self.history_u_1 = []
        self.history_u_2 = []
        self.history_dummy_u_1 = []
        self.history_dummy_u_2 = []
        self.history_raw_1 = []
        self.history_raw_2 = []
        self.history_f = []

    def calc_input(self, x_1, x_2, x_3, time):
        """
        Parameters
        ------------
        x_1 : float
            current state
        x_2 : float
            current state
        x_3 : float
            current state
        time : float in seconds
            now time
        Returns
        --------
        u_1s : list of float
            estimated optimal system input
        u_2s : list of float
            estimated optimal system input
        """
        # calculating sampling time
        dt = self.tf * (1. - np.exp(-self.alpha * time)) / float(self.N)

        # x_dot
        x_1_dot = self.simulator.func_x_1(x_1, x_2,  x_3, self.u_1s[0], self.u_2s[0])
        x_2_dot = self.simulator.func_x_2(x_1, x_2, x_3, self.u_1s[0], self.u_2s[0])
        x_3_dot = self.simulator.func_x_3(x_1, x_2, x_3, self.u_1s[0], self.u_2s[0])

        dx_1 = x_1_dot * self.ht
        dx_2 = x_2_dot * self.ht
        dx_3 = x_3_dot * self.ht

        x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s = self.simulator.calc_predict_and_adjoint_state(x_1 + dx_1, x_2 + dx_2,  x_3 + dx_3, self.u_1s, self.u_2s, self.N, dt)
        
        # Fxt
        Fxt = self._calc_f(x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s, self.u_1s, self.u_2s, self.dummy_u_1s,  self.dummy_u_2s,
                            self.raw_1s, self.raw_2s, self.N, dt)

        # F
        x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s = self.simulator.calc_predict_and_adjoint_state(x_1, x_2, x_3, self.u_1s, self.u_2s, self.N, dt)

        F = self._calc_f(x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s, self.u_1s, self.u_2s, self.dummy_u_1s,  self.dummy_u_2s,
                            self.raw_1s, self.raw_2s, self.N, dt)

        right = -self.zeta * F - ((Fxt - F) / self.ht)

        du_1 = self.u_1s * self.ht
        du_2 = self.u_2s * self.ht
        ddummy_u_1 = self.dummy_u_1s * self.ht
        ddummy_u_2 = self.dummy_u_2s * self.ht
        draw_1 = self.raw_1s * self.ht
        draw_2 = self.raw_2s * self.ht

        x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s = self.simulator.calc_predict_and_adjoint_state(x_1 + dx_1, x_2 + dx_2, x_3 + dx_3, self.u_1s + du_1, self.u_2s + du_2, self.N, dt)

        Fuxt = self._calc_f(x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s, self.u_1s + du_1, self.u_2s + du_2, self.dummy_u_1s + ddummy_u_1,  self.dummy_u_2s + ddummy_u_2, 
                           self.raw_1s + draw_1, self.raw_2s + draw_2, self.N, dt)

        left = ((Fuxt - Fxt) / self.ht)

        # calculationg cgmres
        r0 = right - left
        r0_norm = np.linalg.norm(r0)
        
        vs = np.zeros((self.max_iteration, self.max_iteration + 1)) # 数×iterarion回数
        
        vs[:, 0] = r0 / r0_norm # 最初の基底を算出

        hs = np.zeros((self.max_iteration + 1, self.max_iteration + 1))

        e = np.zeros((self.max_iteration + 1, 1)) # in this case the state is 3(u and dummy_u)
        e[0] = 1.

        for i in range(self.max_iteration):
            du_1 = vs[::self.input_num, i] * self.ht
            du_2 = vs[1::self.input_num, i] * self.ht
            ddummy_u_1 = vs[2::self.input_num, i] * self.ht
            ddummy_u_2 = vs[3::self.input_num, i] * self.ht            
            draw_1 = vs[4::self.input_num, i] * self.ht
            draw_2 = vs[5::self.input_num, i] * self.ht

            x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s = self.simulator.calc_predict_and_adjoint_state(x_1 + dx_1, x_2 + dx_2, x_3 + dx_3, self.u_1s + du_1, self.u_2s + du_2, self.N, dt)

            Fuxt = self._calc_f(x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s, self.u_1s + du_1, self.u_2s + du_2, self.dummy_u_1s + ddummy_u_1,  self.dummy_u_2s + ddummy_u_2, 
                           self.raw_1s + draw_1, self.raw_2s + draw_2, self.N, dt)

            Av = (( Fuxt - Fxt) / self.ht)

            sum_Av = np.zeros(self.max_iteration)

            for j in range(i + 1): # グラムシュミットの直交化法です、和を取って差分を取って算出します
                hs[j, i] = np.dot(Av, vs[:, j])
                sum_Av = sum_Av + hs[j, i] * vs[:, j]

            v_est = Av - sum_Av

            hs[i+1, i] = np.linalg.norm(v_est)

            vs[:, i+1] = v_est / hs[i+1, i]

            inv_hs = np.linalg.pinv(hs[:i+1, :i]) # この辺は教科書(実時間の方)にのっています
            ys = np.dot(inv_hs, r0_norm * e[:i+1])

            judge_value = r0_norm * e[:i+1] - np.dot(hs[:i+1, :i], ys[:i])

            if np.linalg.norm(judge_value) < self.threshold or i == self.max_iteration-1:
                update_value = np.dot(vs[:, :i-1], ys_pre[:i-1]).flatten()
                du_1_new = du_1 + update_value[::self.input_num]
                du_2_new = du_2 + update_value[1::self.input_num]
                ddummy_u_1_new = ddummy_u_1 + update_value[2::self.input_num]
                ddummy_u_2_new = ddummy_u_2 + update_value[3::self.input_num]
                draw_1_new = draw_1 + update_value[4::self.input_num]
                draw_2_new = draw_2 + update_value[5::self.input_num]
                break

            ys_pre = ys
        
        # update
        self.u_1s += du_1_new * self.ht
        self.u_2s += du_2_new * self.ht
        self.dummy_u_1s += ddummy_u_1_new * self.ht
        self.dummy_u_2s += ddummy_u_2_new * self.ht
        self.raw_1s += draw_1_new * self.ht
        self.raw_2s += draw_2_new * self.ht

        x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s = self.simulator.calc_predict_and_adjoint_state(x_1, x_2, x_3, self.u_1s, self.u_2s, self.N, dt)

        F = self._calc_f(x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s, self.u_1s, self.u_2s, self.dummy_u_1s,  self.dummy_u_2s,
                         self.raw_1s, self.raw_2s, self.N, dt)

        print("check F = {0}".format(np.linalg.norm(F)))

        # for save
        self.history_f.append(np.linalg.norm(F))
        self.history_u_1.append(self.u_1s[0])
        self.history_u_2.append(self.u_2s[0])
        self.history_dummy_u_1.append(self.dummy_u_1s[0])
        self.history_dummy_u_2.append(self.dummy_u_2s[0])
        self.history_raw_1.append(self.raw_1s[0])
        self.history_raw_2.append(self.raw_2s[0])

        return self.u_1s, self.u_2s

    def _calc_f(self, x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s, u_1s, u_2s, dummy_u_1s, dummy_u_2s, raw_1s, raw_2s, N, dt):
        """
        Parameters
        ------------
        x_1s : list of float
            predicted x_1s for N steps
        x_2s : list of float
            predicted x_2s for N steps
        x_3s : list of float
            predicted x_3s for N steps
        lam_1s : list of float
            adjoint state of x_1s, lam_1s for N steps
        lam_2s : list of float
            adjoint state of x_2s, lam_2s for N steps
        lam_3s : list of float
            adjoint state of x_2s, lam_3s for N steps
        u_1s : list of float
            estimated optimal system input
        u_2s : list of float
            estimated optimal system input
        dummy_u_1s : list of float
            estimated dummy input
        dummy_u_2s : list of float
            estimated dummy input
        raw_1s : list of float
            estimated constraint variable
        raw_2s : list of float
            estimated constraint variable
        N : int
            predict time step
        dt : float
            sampling time of system
        """
        F = []

        for i in range(N):
            F.append(u_1s[i] + lam_1s[i] * math.cos(x_3s[i]) + lam_2s[i] * math.sin(x_3s[i]) + 2 * raw_1s[i] * u_1s[i])
            F.append(u_2s[i] + lam_3s[i] + 2 * raw_2s[i] * u_2s[i])
            F.append(-0.01 + 2. * raw_1s[i] * dummy_u_1s[i])
            F.append(-0.01 + 2. * raw_2s[i] * dummy_u_2s[i])
            F.append(u_1s[i]**2 + dummy_u_1s[i]**2 - 1.**2)
            F.append(u_2s[i]**2 + dummy_u_2s[i]**2 - 1.5**2)
        
        return np.array(F)

def circle_make_with_angles(center_x, center_y, radius, angle):
    '''
    Create circle matrix with angle line matrix
    
    Parameters
    -------
    center_x : float
        the center x position of the circle
    center_y : float
        the center y position of the circle
    radius : float
    angle : float [rad]
    
    Returns
    -------
    circle xs : numpy.ndarray
    circle ys : numpy.ndarray
    angle line xs : numpy.ndarray
    angle line ys : numpy.ndarray
    '''

    point_num = 100 # 分解能

    circle_xs = []
    circle_ys = []

    for i in range(point_num + 1):
        circle_xs.append(center_x + radius * math.cos(i*2*math.pi/point_num))
        circle_ys.append(center_y + radius * math.sin(i*2*math.pi/point_num))

    angle_line_xs = [center_x, center_x + math.cos(angle) * radius]
    angle_line_ys = [center_y, center_y + math.sin(angle) * radius]

    return np.array(circle_xs), np.array(circle_ys), np.array(angle_line_xs), np.array(angle_line_ys)

def main():
    # simulation time
    dt = 0.01
    iteration_time = 15.
    iteration_num = int(iteration_time/dt)

    # plant
    plant_system = TwoWheeledSystem(init_x_1=-4.5, init_x_2=1.5, init_x_3=0.25)

    # controller
    controller = NMPCController_with_CGMRES()

    # for i in range(iteration_num)
    for i in range(1, iteration_num):
        time = float(i) * dt
        x_1 = plant_system.x_1
        x_2 = plant_system.x_2
        x_3 = plant_system.x_3
        # make input
        u_1s, u_2s = controller.calc_input(x_1, x_2, x_3, time)
        # update state
        plant_system.update_state(u_1s[0], u_2s[0])
    
    # figure
    # time history
    fig_p = plt.figure()
    fig_u = plt.figure()
    fig_f = plt.figure()

    # traj
    fig_t = plt.figure()
    fig_traj = fig_t.add_subplot(111)
    fig_traj.set_aspect('equal')

    x_1_fig = fig_p.add_subplot(311)
    x_2_fig = fig_p.add_subplot(312)
    x_3_fig = fig_p.add_subplot(313)

    u_1_fig = fig_u.add_subplot(411)
    u_2_fig = fig_u.add_subplot(412)
    dummy_1_fig = fig_u.add_subplot(413)
    dummy_2_fig = fig_u.add_subplot(414)

    raw_1_fig = fig_f.add_subplot(311)
    raw_2_fig = fig_f.add_subplot(312)
    f_fig = fig_f.add_subplot(313)

    x_1_fig.plot(np.arange(iteration_num)*dt, plant_system.history_x_1)
    x_1_fig.set_xlabel("time [s]")
    x_1_fig.set_ylabel("x_1")
    
    x_2_fig.plot(np.arange(iteration_num)*dt, plant_system.history_x_2)
    x_2_fig.set_xlabel("time [s]")
    x_2_fig.set_ylabel("x_2")

    x_3_fig.plot(np.arange(iteration_num)*dt, plant_system.history_x_3)
    x_3_fig.set_xlabel("time [s]")
    x_3_fig.set_ylabel("x_3")
    
    u_1_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_u_1)
    u_1_fig.set_xlabel("time [s]")
    u_1_fig.set_ylabel("u_v")

    u_2_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_u_2)
    u_2_fig.set_xlabel("time [s]")
    u_2_fig.set_ylabel("u_omega")

    dummy_1_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_dummy_u_1)
    dummy_1_fig.set_xlabel("time [s]")
    dummy_1_fig.set_ylabel("dummy u_1")

    dummy_2_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_dummy_u_2)
    dummy_2_fig.set_xlabel("time [s]")
    dummy_2_fig.set_ylabel("dummy u_2")

    raw_1_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_raw_1)
    raw_1_fig.set_xlabel("time [s]")
    raw_1_fig.set_ylabel("raw_1")

    raw_2_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_raw_2)
    raw_2_fig.set_xlabel("time [s]")
    raw_2_fig.set_ylabel("raw_2")

    f_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_f)
    f_fig.set_xlabel("time [s]")
    f_fig.set_ylabel("optimal error")

    fig_traj.plot(plant_system.history_x_1, plant_system.history_x_2, color="b", linestyle="dashed")
    fig_traj.set_xlabel("x [m]")
    fig_traj.set_ylabel("y [m]")

    write_obj_num = 5
    count_num = int(iteration_num / write_obj_num)

    for i in np.arange(0, iteration_num, count_num):
        obj_xs, obj_ys, obj_line_xs, obj_line_ys = circle_make_with_angles(plant_system.history_x_1[i], plant_system.history_x_2[i], 0.5, plant_system.history_x_3[i])
        fig_traj.plot(obj_xs, obj_ys, color="k")
        fig_traj.plot(obj_line_xs, obj_line_ys, color="k")

    fig_p.tight_layout()
    fig_u.tight_layout()
    fig_f.tight_layout()

    plt.show()


if __name__ == "__main__":
    main()