import show
import numpy as np
import matplotlib.pyplot as plt
# k = 100
zuolun = 0.01
youlun = 0.01


def geterror(p1, p2, fangxian):
    # 获得误差的函数，包括距离误差和角度误差

    dis = np.linalg.norm(p2-p1)-2  # 计算车与目标的距离
    d2 = p2 - p1  # 计算车与目标的偏差向量
    jiaodu1 = np.arctan(d2[1]/d2[0])/3.14*180  # 计算目标角度

    d2e = jiaodu1 - fangxian  # 计算角度误差
    if d2e > 180:
        d2e -= 180
    if d2e < -180:
        d2e += 180
    return dis, d2e


def control(error, de):
    # pid控制算法误差，输入距离误差和角度误差，输出两个车轮的控制变量

    global zuolun, youlun

    p1 = 0.5
    i1 = 0.00
    p2 = 5
    i2 = 0.0000

    # 计算速度和偏移
    sudo = p1 * de[0] + error[0] * i1
    change = (p2 * de[1] + error[1] * i2)/20

    # 限制，防止转弯过大和加速过快
    if change > 3:
        change = 3
    if change < -3:
        change = -3

    if sudo > 5:
        sudo = 5
    if sudo < -5:
        sudo = -5

    zuolun = sudo + change
    youlun = sudo - change

    if zuolun < youlun:
        zzl = 1

    if zuolun > 10:
        zuolun = 10
    if youlun > 10:
        youlun = 10
    if zuolun < -10:
        zuolun = -10
    if youlun < -10:
        youlun = -10

    return zuolun, youlun


def model(carcontral, p1, p2, fangxiangp1, carability):
    # 小车的运行仿真函数，不细说

    juli = carcontral / 100
    dis = juli[1] - juli[0]

    fangxiangp1 = fangxiangp1 - np.arctan(dis/Trackdistance)/3.14*180
    # print(fangxiangp1)
    fangxiangp1t = np.concatenate(
        (np.cos(fangxiangp1*3.14/180), np.sin(fangxiangp1*3.14/180)))
    julichang = fangxiangp1t * carability / 100

    # print(julichang)
    # if error[0] > 0:

    p1 = p1 + julichang
    # else:
    #     p1 = p1 - julichang

    return p1, p2, fangxiangp1
    # p1 =


Trackdistance = 1  # 轮子之间的距离
quality = 1  # 质量
carability = 7  # 车轮最大输出速度


# 简单的参数
data = [10]
data1 = np.random.random_sample(size=(2, 400)) * 10
data2 = np.random.random_sample(size=(2, 400)) * 10

start1 = np.array([0, 0])
start2 = np.array([3, 0])

speed = np.array([0.5, 0.5])
speed1 = np.array([0.5, -0.5])
direction = np.array(0)


# 角度制
fangxiangp1 = np.array([0])


data1[:, 0] = start1
data2[:, 0] = start2

p1 = start1
p2 = start2
de = [0, 0]

error1 = []
error2 = []
carcontral1 = []

# 主循环
for i in range(399):
    print('--------------------------------')
    # p1为小车
    p1 = p1
    # p2为目标

    # 定义目标的运行规律，随机函数加上一个轨迹函数
    if i < 200:
        p2 = p2 + speed/10 + (np.random.random_sample((2))-0.5)/5
    else:
        p2 = p2 + speed1/10 + (np.random.random_sample((2))-0.5)/5


    # 计算误差
    error = geterror(p1, p2, fangxiangp1)
    # 储存误差参数
    error1.append(error[0])
    error2.append(error[1])
    print('error', error)
    
    # 计算控制量
    carcontral = control(error, de)
    # 储存控制量的参数
    carcontral1.append(carcontral)
    print('carcontral', carcontral)
    carcontral = np.array(carcontral)
    # 记录当前误差，用于下次循环
    de = error
    
    # 车辆运行
    p1, p2, fangxiangp1 = model(carcontral, p1, p2, fangxiangp1, carability)
    print('p1p2', p1, p2)
    print('fangxiang', fangxiangp1)

    if fangxiangp1 > 180:
        fangxiangp1 = fangxiangp1 - 180
    if fangxiangp1 < -180:
        fangxiangp1 = fangxiangp1 + 180

    data1[:, i+1] = p1
    data2[:, i+1] = p2


show.main(data1, data2)