#!/usr/bin/python2.7
#-*- encoding: UTF-8 -*-
import vision_definitions
#----------------------單目測距--------------------------------
#***********************************************
#@函數名：   DistAndDirect_cal(cxnum,rynum,colsum,rowsum,Head_angle,cameraID)
#@參數：     (cxnum,rynum)是通過圖像識別得到球心的像素點坐標
#           (colsum,rowsum)是圖片總大?。?40*480
#            cameraID=0，取上攝像頭；cameraID=1，取下攝像頭
#@返回值：   無
#@功能說明： 采用機器人的下攝像頭進行測量球離機器人的相關角度與距離
def DistAndDirect_cal(cxnum,rynum,colsum,rowsum,Head_angle,cameraID):
    distx=-(cxnum-colsum/2)
    disty=rynum-rowsu/2
    print distx,disty
    Picture_angle=disty*47.64/480

    if cameraID ==0:
        h=0.62
        Camera_angle=12
    else:
        h=0.57
        Camera_angle=38
    #Head_angle[0]機器人仰俯角度
    Total_angle=math.pi*(Picture_angle+Camera_angle)/180+Head_angle[0]
    d1=h/math.tan(Total_angle)

    alpha=math.pi*(distx*60.92/640)/180
    d2=d1/math.cos(alpha)
    #Head_angle[1]機器人左右角度
    Forward_Distance=d2*math.cos(alpha+Head_angle[1])
    Sideward_Distance=-d2*math.sin(alpha+Head_angle[1])
#***********************************************

#@函數名：   GetNaoImage(IP,PORT,cameraID)
#@參數：     略
#@返回值：   無
#@功能說明： 采調用機器人內置攝像頭控制模塊，對當前場景進行拍攝并保持。
#           由于球距離機器人約小于0.6m時，機器人額頭攝像頭無法看到，
#           所以需要變換攝像頭，cameraID=0，取上攝像頭；
#           cameraID=1，取下攝像頭
def Get NaoImage(IP,PORT,cameraID):
    camProxy=ALProxy("ALVideoDevice",IP,PORT)
    resolition=2 #VGA格式640*480
    colorSpace=11#RGB

    #選擇并啟用攝像頭
    camProxy.setParam(vision_definitions.kCameraSelectID,cameraID)
    videoClient=camProxy.subscribe("python_client",resolition,colorSpace,5)

    #獲取攝像機圖像。
    #image [6]包含以ASCII字符數組形式傳遞的圖像數據。
    naoImage=camProxy.getImageRemote(videoClient)

    camProxy.unsubscribe(videoClient)
    #獲取圖像大小和像素陣列。
    imageWidth=naoImage[0]
    imageHeight=naoImage[1]
    array=naoImage[6]
    #從我們的像素陣列創建一個PIL圖像。
    im=Image.fromstring("RGB",(imageWidth,imageHeight),array)
    #保存圖像。
    im.save("temp.jpg","JPEG")

#***********************************************
#@函數名：   findColorPattern(img,pattern)
#@參數：     略
#@返回值：   無
#@功能說明：  將RGB圖像轉化為二值圖：此方法用的是cv,可以嘗試用cv2代碼會更加簡潔
def  findColorPattern(img,pattern):
    channels=[None,None,None]
    channels[0]=cv.CreateImage(cv.GetSize(img),8,1)
    channels[1]=cv.CreateImage(cv.GetSize(img),8,1)
    channels[2]=cv.CreateImage(cv.GetSize(img),8,1)
    ch0=cv.CreateImage(cv.GetSize(img),8,1)
    ch1=cv.CreateImage(cv.GetSize(img),8,1)
    ch2=cv.CreateImage(cv.GetSize(img),8,1)
    cv.Split(img,ch0,ch1,ch2,None)
    dest=[None,None,None,None]
    dest[0]=cv.CreateImage(cv.GetSize(img),8,1)
    dest[1]=cv.CreateImage(cv.GetSize(img),8,1)
    dest[2]=cv.CreateImage(cv.GetSize(img),8,1)
    dest[3]=cv.CreateImage(cv.GetSize(img),8,1)
    cv.Smooth(ch0,channels[0],cv.CV_GAUSSIAN,3,3,0)
    cv.Smooth(ch1,channels[1],cv.CV_GAUSSIAN,3,3,0)
    cv.Smooth(ch2,channels[2],cv.CV_GAUSSIAN,3,3,0)
    for i in range(3):
        k=2-i
        lower=pattern[k]-75#設置閾值
        upper=pattern[k]+75
        cv.InRangeS(channels[i],lower,upper,dest[i])

    cv.And(dest[0],dest[1],dest[3])
    temp=cv.CreateImage(cv.GetSize(img),8,1)
    cv.And(dest[2],dest[3],temp)
    '''
    cv.NameWindow("result",cv.CV_WINDOW_AUTOSIZE)
    cv.ShowImage("result",temp)
    cv.WaitKey(0)
    '''
    return temp

#***********************************************
#@函數名：   xyProject(matrix,imgaesize)
#@參數：     matrix
#           imgaesize
#@返回值：   無
#@功能說明： 利用二值圖，計算球的像素坐標。其原理是：遍歷各行各列
#           像素的數值的和，最大的組合即為球心坐標
def xyProject(matrix,imagesize):
    #聲明一個數據類型為8位型單通道的imagessize[1]*1/1*imagessize[0]矩陣(初始值為 0)。
    colmask=cv.CreateMat(imagessize[1],1,cv.CV_8UC1)
    rowmask=cv.CreateMat(1,imagessize[0],cv.CV_8UC1)
    cv.Set(colmask,1)
    cv.Set(rowmask,1)

    colsum=[]
    for i in range(imagesize[0]):
        col=cv.GetCol(matrix,i)
        #計算向量點積
        a=cv.DotProduct(colmask,col)
        colsum.append(a)

    rowsum=[]
    for i in range(imagesize[1]):
        row=cv.GetRow(matrix,i)
        a=cv.DotProduct(rowmask,row)
        rowsum.append(a)

    return(colsum,rowsum)#得到各行各列“1”值的和

def crMax(colsum,rowsum):
    cx=max(colsum)
    ry=max(rowsum)
    for i in range(len(colsum)):
        if colsum[i]==cx:
            cxnum=i
    for i in range(len(rowsum)):
        if rowsum[i]==ry:
            rynum=i
    return(cxnum,rynum)
#***********************************************
#@函數名：  GetHeadAngles(robotIP,PORT)
#@參數：    略
#@返回值：   無
#@功能說明：
def GetHeadAngles(robotIP,PORT):
    motionProxy=ALProxy("ALMotion",robotIP,PORT)
    names=["HeadPitch","HeadYaw"]
    useSensors=1
    sensorAngles=motionProxy.getAngles(names,useSensors)
    return sensorAngles
#***********************************************
#@函數名：  SetHeadAngles(robotIP,PORT,angles)
#@參數：    略
#@返回值：   無
#@功能說明：
def SetHeadAngles(robotIP,PORT,angles):
    motionProxy=ALProxy("ALMotion",robotIP,PORT)
    motionProxy.setStiffnesses("Head",1.0)

    names=["HeadPitch","HeadYaw"]
    fractionMaxSpeed=0.2
    motionProxy.setAngles(names,angles,fractionMaxSpeed)

    time.sleep(2.0)
    motionProxy.setStiffnesses("Head",0.0)

#***********************************************
#@函數名：   Capture_Picture(IP,PORT,cameraID,angles,pattern_colors)
#@參數：     angles
#           pattern_colors
#@返回值：   無
#@功能說明： 將上面的一系列函數整合起來

def Capture_Picture(IP,PORT,cameraID,angles,pattern_colors):
    SetHeadAngles(IP,PORT,angles)
    GetNaoImage(IP,PORT,cameraID)
    image=cv.LoadImage("temp.jpg")
    imagesize=cv.GetSize(image) #返回數值，兩個元素分別為列數和行數
    matrix=findColorPattern(image,pattern_colors)
    (colsum,rowsum)=xyProject(matrix,imagesize)
    (cxnum,rynum)=crMax(colsum,rowsum)
    cv.SaveImage("result.jpg",matrix)

    return (cxnum,rynum,colsum,rowsum)

 

#***********************************************
#@函數名：   Target_Detect_and_Distance(IP,PORT)
#@參數：
#@返回值：   無
#@功能說明： 當上攝像頭無法找到球時，切換到下攝像頭，然后在左轉右轉。
#       在這個過程中，如果發現目標，則計算距離并輸出距離
#       若始終未找到目標，則輸出距離為0。

def Target_Detect_and_Distance(IP,PORT):
    pattern_colors=(255,150,50)
    cameraID=0# 默認上攝像頭
    angles=[0,0]
    (cxnum,rynum,colsum,rowsum)=Capture_Picture(IP,PORT,cameraID,angles)

    if(cxnum,rynum)==(639,479):
        cameraID=1
        (cxnum,rynum,colsum,rowsum)=Capture_Picture(IP,PORT,cameraID,angles)
    if(cxnum,rynum)==(639,479):
        cameraID=0
        angles=[0.0.7]
        (cxnum,rynum,colsum,rowsum)=Capture_Picture(IP,PORT,cameraID,angles)
    if(cxnum,rynum)==(639,479):
        cameraID=0
        angles=[0,-0.7]
        (cxnum,rynum,colsum,rowsum)=Capture_Picture(IP,PORT,cameraID,angles)
    HeadAngles-GetHeadAngles(IP,PORT)
    ###############
    (Forward_Distance,Sideward_Distance)=DistAndDirect_cal(cxnum,rynum,colsum,rowsum,Head_angle,cameraID)
    if(cxnum,rynum)==(639,479):
        (Forward_Distance,Sideward_Distance)=(0,0)
    print "Forward_Distance=",Forward_Distance,"meters"
    print "Sideward_Distance="+Sideward_Distance+"meters"

#***********************************************
#@函數名：   Target_Detect_and_Distance(IP,PORT)
#@參數：
#@返回值：   無
#@功能說明： 當找到球后，可能會存在一定的誤差。
#           因此需要判斷球位于機器人前方的哪一側，再來確定用哪只腳踢球

def Final_See(robotIP,PORT):
    pattern_colors=(255,150,50)
    angles=[0.5,0]
    SetHeadAngles(robotIP,PORT,angles)

    cameraID=1

    GetNaoImage(robotIP,PORT,cameraID)
    image=cv.LoadImage("temp.jpg")
    imagesize=cv.GetNaoImage(image)

    matrix=findColorPattern(image,pattern_colors)

    (colsum,rowsum)=xyProject(matrix,imgaesize)
    (cxnum,rynum)=crMax(colsum,rowsum)
    cv.SaveImage("result.jpg",matrix)

    HeadAngles=GetHeadAngles(robotIP,PORT)
    #########################
    (Forward_Distance,Sideward_Distance)=DistAndDirect_cal(cxnum,rynum,colsum,rowsum,Head_angle,cameraID)

    if cxnumlen(colsum)/2:
        side=0#左腳
    else:
        side=1#右腳
    print "side=",side
    print "last distance=",Forward_Distance
    return (side,Forward_Distance)