import numpy as np

from computeRotationMatrix import computeRotationMatrix
from basicLocalGeoTransfo import computeUbar, computeTransfoBasicToLocal
from constitutiveModels import trilinearResponse
from secStateDetermin import sectionStateDetermin_uniaxialFiber

def elemStateDetermin(elemIdx,elem,numEquations, v):
    dofs_elem=numEquations[elemIdx]

    T=computeRotationMatrix(elem)
    u=np.dot(T,v[dofs_elem])

    if elem[0]=='2dElasticBeam':
        qBar, KBar=elemStateDetermin_2dElasticBeam(elem,u)

        l=np.linalg.norm(elem[3]-elem[2])
        LTransfo, Kgeom=computeTransfoBasicToLocal(elem[7],l,u,qBar)

        QLocal_elem = LTransfo.T @ qBar
        QGlobal_elem = T.T @ QLocal_elem
        KMaterialGlobal_elem = T.T  @ LTransfo.T @ KBar @ LTransfo @ T
        KGeomGlobal_elem = T.T @ Kgeom @ T

    elif elem[0]=='spring':
        elem, QLocal_elem, KLocal=elemStateDetermin_spring(elem,u)
        QGlobal_elem = QLocal_elem
        KMaterialGlobal_elem = KLocal
        KGeomGlobal_elem = np.zeros((2,2))

    elif elem[0]=='2dModElasticBeam':
        qBar, KBar=elemStateDetermin_2dModElasticBeam(elem,u)

        l=np.linalg.norm(elem[3]-elem[2])
        LTransfo, Kgeom=computeTransfoBasicToLocal(elem[11],l,u,qBar)

        QLocal_elem = LTransfo.T @ qBar
        QGlobal_elem = T.T @ QLocal_elem
        KMaterialGlobal_elem = T.T  @ LTransfo.T @ KBar @ LTransfo @ T
        KGeomGlobal_elem = T.T @ Kgeom @ T

    elif elem[0]=='2dDispBeamColumn':
        qBar, KBar=elemStateDetermin_2dDispBeamColumn(elem,u)

        l=np.linalg.norm(elem[3]-elem[2])
        LTransfo, Kgeom=computeTransfoBasicToLocal(elem[9],l,u,qBar)

        QLocal_elem = LTransfo.T @ qBar
        QGlobal_elem = T.T @ QLocal_elem
        KMaterialGlobal_elem = T.T  @ LTransfo.T @ KBar @ LTransfo @ T
        KGeomGlobal_elem = T.T @ Kgeom @ T


    return elem, QGlobal_elem, KMaterialGlobal_elem, KGeomGlobal_elem, QLocal_elem   



def elemStateDetermin_2dElasticBeam(elem,u):
    l=np.linalg.norm(elem[3]-elem[2])
    E=elem[4]
    A=elem[5]
    I=elem[6]
    KBar=np.array([
    [E * A / l, 0, 0],
    [0, 4 * E * I / l, 2 * E * I / l],
    [0, 2 * E * I / l, 4 * E * I / l]])

    uBar=computeUbar(elem[7],l,u)

    qBar=np.dot(KBar,uBar)

    return qBar, KBar


def elemStateDetermin_2dModElasticBeam(elem,u):
    E=elem[4]
    A=elem[5]
    Ie=elem[10]
    l=np.linalg.norm(elem[3]-elem[2])
    S22mod=elem[6]
    S23mod=elem[7]
    S32mod=elem[8]
    S33mod=elem[9]

    KBar=np.array([
    [E * A / l, 0, 0],
    [0, S22mod * E * Ie / l, S23mod * E * Ie / l],
    [0, S32mod * E * Ie / l, S33mod * E * Ie / l]])

    uBar=computeUbar(elem[11],l,u)

    qBar=np.dot(KBar,uBar)

    return qBar, KBar


def elemStateDetermin_spring(elem,u):
    # Compute spring rotation
    theta = u[1]-u[0]

    # Unpack the spring material properties
    spring_MatProps_input=elem[3]
    theta_previous_input = spring_MatProps_input[0]
    M_previous_input = spring_MatProps_input[1]
    theta_M0_currentPos_input = spring_MatProps_input[2]
    theta_M0_currentNeg_input = spring_MatProps_input[3]
    theta_M0_projected_input = spring_MatProps_input[4]
    ke_s_input = spring_MatProps_input[5]
    ks_s_input = spring_MatProps_input[6]
    kpc_s_input = spring_MatProps_input[7]
    thetaY_s_input = spring_MatProps_input[8]
    thetaC_s_input = spring_MatProps_input[9]
    thetaU_s_input = spring_MatProps_input[10]
    My_input = spring_MatProps_input[11]
    Mu_input = spring_MatProps_input[12]
    MmaxPos_input = spring_MatProps_input[13]
    MmaxNeg_input = spring_MatProps_input[14]
    yieldFlag_Pos_input = spring_MatProps_input[15]
    cappingFlag_Pos_input = spring_MatProps_input[16]
    yieldFlag_Neg_input = spring_MatProps_input[17]
    cappingFlag_Neg_input = spring_MatProps_input[18]
    reversalFlag_input = spring_MatProps_input[19]
    residualFlag_input = spring_MatProps_input[20]
    Di_previous_input = spring_MatProps_input[21]

    #Call the constitutive law for the spring
    result = trilinearResponse(
    theta,
    theta_previous_input,
    M_previous_input,
    theta_M0_currentPos_input,
    theta_M0_currentNeg_input,
    theta_M0_projected_input,
    ke_s_input,
    ks_s_input,
    kpc_s_input,
    thetaY_s_input,
    thetaC_s_input,
    thetaU_s_input,
    My_input,
    Mu_input,
    MmaxPos_input,
    MmaxNeg_input,
    yieldFlag_Pos_input,
    cappingFlag_Pos_input,
    yieldFlag_Neg_input,
    cappingFlag_Neg_input,
    reversalFlag_input,
    residualFlag_input,
    Di_previous_input
)
    
    # Unpack the results
    (
        M, k, MmaxPos, MmaxNeg, yieldFlag_Pos, cappingFlag_Pos,
        yieldFlag_Neg, cappingFlag_Neg, reversalFlag, residualFlag,
        theta_M0_currentPos, theta_M0_currentNeg, theta_M0_projected, Di
    ) = result
    

    # Spring resisting force and tangent stiffness 
    QLocal=np.array([-M,M])
    KLocal=np.array([[k,-k],[-k,k]])
    # #Test sign
    # Qtest=KLocal@u

    # Assign the output variables
    spring_MatProps_Output = np.array([
    theta,           # Updated theta value (current rotation)
    M,                     # Updated moment
    theta_M0_currentPos,   # Updated theta_M0_currentPos
    theta_M0_currentNeg,   # Updated theta_M0_currentNeg
    theta_M0_projected,    # Updated theta_M0_projected
    ke_s_input,            # Stiffness values remain the same
    ks_s_input,
    kpc_s_input,
    thetaY_s_input,
    thetaC_s_input,
    thetaU_s_input,
    My_input,
    Mu_input,
    MmaxPos,               # Updated MmaxPos
    MmaxNeg,               # Updated MmaxNeg
    yieldFlag_Pos,         # Updated yieldFlag_Pos
    cappingFlag_Pos,       # Updated cappingFlag_Pos
    yieldFlag_Neg,         # Updated yieldFlag_Neg
    cappingFlag_Neg,       # Updated cappingFlag_Neg
    reversalFlag,          # Updated reversalFlag
    residualFlag,          # Updated residualFlag
    Di                     # Updated direction increment
])
    elem[4]=spring_MatProps_Output

    return elem, QLocal, KLocal


def elemStateDetermin_2dDispBeamColumn(elem,u):
    l=np.linalg.norm(elem[3]-elem[2])

    uBar=computeUbar(elem[9],l,u)

    xi=elem[4] #x/l
    wt=elem[5] #weight
    wtL=wt*l
    numSection=len(xi)

    # Initialize element state variables
    qBar=np.zeros(3)
    KBar=np.zeros((3,3))

    # Loop over the integration sections
    for iSec in range(numSection):
        Bbar=np.array([[1/l, 0, 0],
                       [0, 1/l*(6*xi[iSec]-4), 1/l*(6*xi[iSec]-2)]])
        
        ds_iSec=Bbar@uBar

        # Section state determination
        materialInput=elem[6]
        coordinates_fibers_columns=elem[7]
        area_fibers_columns=elem[8]
        Qsr, ks=sectionStateDetermin_uniaxialFiber(materialInput,coordinates_fibers_columns, area_fibers_columns,ds_iSec)

        # Integrate all sections over the element length
        qBar+=Bbar.T@Qsr*wtL[iSec]
        KBar+=Bbar.T@ks@Bbar*wtL[iSec]

        
    return qBar, KBar

