import numpy as np
import matplotlib.pyplot as plt

#---------------------------------------------------------------------------
#根据时间模式一个角度输出 0-2pi
def angle_creat(num, inc):
    return (np.arange(num)*inc)

def angle_creat_one_sin(num):
    return (np.arange(num) * (2*np.pi) / num)

def creat_sin_table_in_Q15(num):
    sin = []
    for i in range(num):
        sin.append(int(np.sin((np.pi * 2) / num * i) * 32768))
    print(len(sin))
    print(sin)
    return sin
#===========================================================================


#---------------------------------------------------------------------------
def svpwm_alpha_beta_to_Uabc(Ualpha, Ubeta):#svpwm使用
    Ua = Ubeta
    Ub = (  np.sqrt(3) * Ualpha - Ubeta) / 2 #  a - b
    Uc = ( -np.sqrt(3) * Ualpha - Ubeta) / 2 # -a - b
    return Ua, Ub, Uc
#===========================================================================


#---------------------------------------------------------------------------
#clarke转换
def clarke_conversion(Ua, Ub, Uc):#已经做了等幅值变换  转换出来 最大幅值一样
    Ualpha = Ua
    Ubeta = (2 * Ub + Ua) / np.sqrt(3)
    return Ualpha, Ubeta

#clarke逆变换
def clarke_inverse_conversion(Ualpha, Ubeta):
    Ua = Ualpha
    Ub = (-Ualpha + np.sqrt(3) * Ubeta) / 2
    Uc = (-Ualpha - np.sqrt(3) * Ubeta) / 2
    return Ua, Ub, Uc
#===========================================================================




#---------------------------------------------------------------------------
#park转换
def park_conversion(Ualpha, Ubeta, theta):
    Ud =  Ualpha * np.cos(theta) + Ubeta * np.sin(theta)
    Uq = -Ualpha * np.sin(theta) + Ubeta * np.cos(theta)
    return Uq, Ud

#park逆转换
def park_inverse_conversion(in_Uq, in_Ud, theta):
    Ualpha = -in_Uq * np.sin(theta) + in_Ud * np.cos(theta)
    Ubeta  =  in_Uq * np.cos(theta) + in_Ud * np.sin(theta)   
    return Ualpha, Ubeta
#===========================================================================


#---------------------------------------------------------------------------
if __name__ == "__main__":
    #-------------互转----------------------------------------------
    # elec_angle = angle_creat(200, (2*np.pi)/200)
    elec_angle = angle_creat_one_sin(200)
    dem_Iq = 20

    Ualfa, Ubeta = park_inverse_conversion(dem_Iq, 0, elec_angle)
    Ua, Ub, Uc = clarke_inverse_conversion(Ualfa, Ubeta)
    oUa, oUb = clarke_conversion(Ua, Ub, Uc)
    oUq, oUd = park_conversion(oUa, oUb, elec_angle)


    dem_Iq = 20 / 5.12

    Ualfax, Ubetax = park_inverse_conversion(dem_Iq, 0, elec_angle)
    Uax, Ubx, Ucx = clarke_inverse_conversion(Ualfax, Ubetax)
    Uax = Uax * 5.12
    Ubx = Ubx * 5.12
    Ucx = Ucx * 5.12
    # oUax, oUbx = clarke_conversion(Uax, Ubx, Ucx)
    # oUqx, oUdx = park_conversion(oUax, oUbx, elec_angle)
    #================================================================

    #-------sin q15 生成 转换-------
    sintableQ15 = creat_sin_table_in_Q15(512)

    numSin = np.arange(len(sintableQ15))
    plt.figure("Q15")
    plt.plot(numSin, sintableQ15, label='sin')
    plt.ylabel('alpha-beta')
    plt.legend()

    realSin = np.sin(elec_angle)
    #跟定角速度,给出角度增量,通过查表获得sin值
    pwmFrq = 16000
    omega = 20  #rps
    # angleStep = omega / pwmFrq * 2 * np.pi # 角速度(r/s) * 时间(s) = r  (1r = 2pi)
    angleStep = omega / pwmFrq * 512 #这里有小数  需要继续增大512 角速度(r/s) * 时间(s) = r  (1r = 2pi) 0-2pi--->0-512

    oneCircleStep = pwmFrq / omega 

    print("get omega:", omega, "stepMax:", oneCircleStep, "angleStep:", angleStep)
    newAngleTable = np.arange(int(oneCircleStep)) * angleStep
    newAngleTable = newAngleTable.astype(int)
    print(newAngleTable)
    newSinTable = []
    newCosTable = []
    for i in range(int(oneCircleStep)):
        newSinTable.append(sintableQ15[newAngleTable[i]]/32768)
        tmp = newAngleTable[i] + 128
        if tmp >= 512:
            tmp = tmp - 512
        newCosTable.append(sintableQ15[tmp]/32768)

    time = np.arange(int(oneCircleStep))
    plt.figure("Q15 angle")
    plt.plot(time, newSinTable, label='sin')
    plt.plot(time, newCosTable, label='cos')
    plt.ylabel('alpha-beta')
    plt.legend()
    #===============================

    # time = np.arange(len(elec_angle))
    # plt.figure("angle")
    # plt.plot(elec_angle, elec_angle, label='angle')
    # plt.ylabel('alpha-beta')
    # plt.legend()

    # # 绘制波形图
    # plt.figure("Iq Id to alpha-beta")
    # plt.plot(elec_angle, Ualfa, label='Uafla')
    # plt.plot(elec_angle, Ubeta, label='Ubeta')
    # plt.ylabel('alpha-beta')
    # plt.legend()

    # plt.figure('alpha-beta to A B C')
    # plt.plot(elec_angle, Ua, label='A')
    # plt.plot(elec_angle, Ub, label='B')
    # plt.plot(elec_angle, Uc, label='C')
    # plt.ylabel('A B C')
    # plt.legend()

    # plt.figure('alpha-beta x to A B C')
    # plt.plot(elec_angle, Uax, label='A')
    # plt.plot(elec_angle, Ubx, label='B')
    # plt.plot(elec_angle, Ucx, label='C')
    # plt.ylabel('A B C')
    # plt.legend()

    # plt.figure('alpha-beta xa to A B C')
    # plt.plot(elec_angle, Ua, label='A')
    # plt.plot(elec_angle, Uax, label='B')
    # # plt.plot(elec_angle, Ucx, label='C')
    # plt.ylabel('A B C')
    # plt.legend()

    # plt.figure("abc to alpha-beta")
    # plt.plot(elec_angle, oUa, label='oa')
    # plt.plot(elec_angle, oUb, label='ob')
    # plt.ylabel('alpha-beta')
    # plt.legend()

    # plt.figure("alpha-beta to Iq Id")
    # plt.plot(elec_angle, oUq, label='oUq')
    # plt.plot(elec_angle, oUd, label='oUd')
    # plt.ylabel('oUq-oUd')
    # plt.legend()

    # plt.figure()
    # plt.plot(elec_angle, oUq, label='oUq')
    # plt.plot(elec_angle, oUd, label='oUd')
    # plt.ylabel('out q d')
    # plt.legend()

    plt.show()