# Code to check out the effect of different bedslope approximations.


# Compute side length
len<-function(p1, p2){
    sum( (p1[1]-p2[1])**2 + (p1[2]-p2[2])**2)^0.5
}


# Compute normals
nml<-function(p1, p2){
    # This will produce a normal to p2-p1 (positive in the direction rotated
    # left from p2-p1)
    n_parallel = p2[1:2]-p1[1:2]
    n_perp = c(n_parallel[2], -n_parallel[1])
    n_perp_len = sum(n_perp**2)^0.5
    n_perp/n_perp_len
}

# Compute Area
Area<-function(p1,p2,p3){
        n1=nml(p2,p1)
        # 0.5*base*vertical height
        0.5*len(p1,p2)*sum((p3[1:2]-p2[1:2])*n1)^0.5
    }

# z edge values
z_edge<-function(p1,p2){
        0.5*(p2[3]+p1[3])
    }

# z centroid values
z_c<-function(p1,p2,p3){
    (p1[3]+p2[3]+p3[3])/3.0
}


bedslope_difference<-function(water, p1,p2,p3, rescale=1){
    
    # Use rescale to scale the triangle side lengths
    p2 = p1+(p2-p1)*rescale
    p3 = p1 + (p3-p1)*rescale

    # Side lengths
    l1 = len(p1, p2)
    l2 = len(p2, p3)
    l3 = len(p3, p1)

    # Normals
    n1=nml(p1,p2)
    n2=nml(p2,p3)
    n3=nml(p3,p1)

    # Ordinary bedslope
    dzdx=(l1*n1*z_edge(p1,p2) + l2*n2*z_edge(p2,p3) + l3*n3*z_edge(p3,p1))/Area(p1,p2,p3)
    # Centroid depth
    dc = water-z_c(p1,p2,p3)

    # Bedslope 1
    b1=dzdx*g*dc

    # Bedslope 2
    b2=-g/2*(l1*n1*(water-z_edge(p1,p2))**2 + l2*n2*(water-z_edge(p2,p3))**2 + l3*n3*(water-z_edge(p3,p1))**2)/Area(p1,p2,p3)

    # Alternative_depth
    d2= b2/(dzdx*g)

    out=list()
   
    out$standard_bedslope=b1
    out$alternative_bedslope=b2
    out$ratio=b2/b1
    out$centroid_depth=dc
    out$alternative_depth_vector=d2 
    #print(c('Standard bedslope', b1))
    #print(c('Alternative bedslope', b2))
    #print(c('Ratio b2/b1', b2/b1))
    #print(c('Centroid depth: ', dc, ' Alternative depth vector:', d2))
    out

}

#############################3
#
# MAIN COMPUTATION
#
#############################



# Water elevation
water=1.2

# Triangle points
p1 = c(0,1, 0)
p2 = c(0, 0, 1)
p3= c(1,0,0)

# Gravity
g=9.8

out=bedslope_difference(water, p1,p2,p3)

n=100
waters=seq(1.1,10,len=n)
store1=matrix(NA,nrow=n,ncol=2)
storep5=matrix(NA,nrow=n,ncol=2)
storep25=matrix(NA,nrow=n,ncol=2)
for(i in 1:length(waters)){
    out=bedslope_difference(waters[i],p1,p2,p3)
    store1[i,]=out$ratio
    out=bedslope_difference(waters[i],p1,p2,p3, rescale=0.5)
    storep5[i,]=out$ratio
    out=bedslope_difference(waters[i],p1,p2,p3, rescale=0.25)
    storep25[i,]=out$ratio
}