class Program_t:

    def __init__(self,bt):
        self.BT = bt
        self.BT_x = [b[0] for b in self.BT]
        self.BT_y = [b[1] for b in self.BT]
        self.T=[b[2] for b in self.BT]
        self.number = len(bt)
        
    def LS_steps(self):
        num = len(self.T)
        up_control = 2*max(self.T)**2#限制最大元素量級
        Q = cp.Variable((num,num))#待求變量
        tao = cp.Variable((num,1))#生成矩陣形式后面才可以拼接
        y_ = cp.Variable((2,1))
        y_s = cp.Variable((1,1))#矩陣形式用于拼接
        yita = 0.000005*sum(self.T) / num#論文給出的參數選擇，可更改常數
        G = np.eye(num)-np.ones((num,num))
        t = np.array([self.T]).T
        expr1 = cp.trace((cp.transpose(G)) @ G @ (Q- cp.multiply(2,t @ (cp.transpose(tao)))+t @ (cp.transpose(t))))
        expr2 = yita*cp.sum(Q)
        expr = expr1+ expr2#目標函數
        Q_ = cp.bmat([[Q,tao],[cp.transpose(tao),[[1]]]])#拼接矩陣
        Y = cp.bmat([[np.eye(2),y_],[cp.transpose(y_),y_s]])#拼接矩陣
        constraints = [Q_ >> 0, Y >> 0, cp.max(Q)<=up_control]#限制條件Q半正定，Y半正定，Q最大元素小于上限（這個約束非常重要，是我自己加上去的）
        for i in range(num):
            X = np.array([self.BT_x[i],self.BT_y[i],-1]).T
            constraints += [Q[i, i] == cp.transpose(X) @ Y @ X]#約束條件
            for j in range(i+1,num):
                X_j = np.array([self.BT_x[j], self.BT_y[j],-1]).T
                constraints += [Q[i, j] >= cp.abs(cp.transpose(X) @ Y @ X_j)]#約束條件
        obj = cp.Minimize(expr)
        prob = cp.Problem(obj, constraints)
        prob.solve()
        position = y_.value
        print(expr1.value)#輸出值
        print(expr2.value)
        print(prob.value)#輸出值
        print(prob.status)#輸出狀態
        print(position)
        return position