Rotation according to the terrain

I have some progress right here.

def checkCollisions(self):
        self.cTrav.traverse(self.render)
        
        entries = []

        for i in range(self.roboGroundHandler.getNumEntries()):
            entry = self.roboGroundHandler.getEntry(i)
            entries.append(entry)
        entries.sort(lambda x,y: cmp(y.getSurfacePoint(self.render).getZ(),
                                     x.getSurfacePoint(self.render).getZ()))
        if (len(entries)>0) and (entries[0].getIntoNode().getName() == "terrain"):
            self.robo.setZ(entries[0].getSurfacePoint(self.render).getZ()+0.40)     
            #Calcula a rotacao no eixo X
            b = entries[0].getSurfaceNormal(self.render).getX() / fabs(entries[0].getSurfaceNormal(self.render).getX())
            #b=b*-1
            p = fabs(self.robo.getH() % 360)
            hP = 90 - entries[0].getSurfaceNormal(self.render).angleDeg(Vec3(0,1,0))
            hR = 90 - entries[0].getSurfaceNormal(self.render).angleDeg(Vec3(1,0,0))
            if (p >= 0 and p < 90):
                self.robo.setP(hP)
                self.robo.setR(hR)
            if (p >= 90 and p < 180):
                self.robo.setR(hR)
                self.robo.setP(hP)
            if (p >= 180 and p < 270):
                self.robo.setP(360 - hP)
                self.robo.setR(360 - hR)
            if (p >= 270 and p <= 359):
                self.robo.setR(360 - hR)
                self.robo.setP(360 - hP)
                
            print self.robo.getHpr()
            print 90 - entries[0].getSurfaceNormal(self.render).angleDeg(Vec3(0,1,0))
            print 90 - entries[0].getSurfaceNormal(self.render).angleDeg(Vec3(1,0,0))
        else:
            self.robo.setPos(self.bpos)

This is my collision function, some part of it extracted from the roaming ralph example, the rotation is going good but still need some improved