# -*- coding: utf-8 -*-
"""
Created on Tue Jun 13 14:39:54 2017

@author: eleve
"""

from naoqi import ALProxy
import time

record = ALProxy("ALAudioRecorder", "169.254.28.144", 9559)
record = None

def soundRecord(robot_IP, robot_PORT=9559):
    global record
    record = ALProxy("ALAudioRecorder", "169.254.28.144", 9559)
    record_path = '/home/nao/danseavec.wav'
    record.stopMicrophonesRecording()
    record.startMicrophonesRecording(record_path, 'wav', 16000, (1,0,0,0))
    time.sleep(5)
    record.stopMicrophonesRecording()

import sys
from numpy import NaN, Inf, arange, isscalar, asarray, array, size

def peakdet(v, delta, x = None):

    maxtab = []
    mintab = []

    if x is None:
        x = arange(len(v))

    v = asarray(v)

    if len(v) != len(x):
        sys.exit('Input vectors v and x must have same length')

    if not isscalar(delta):
        sys.exit('Input argument delta must be a scalar')

    if delta <= 0:
        sys.exit('Input argument delta must be positive')

    mn, mx = Inf, -Inf
    mnpos, mxpos = NaN, NaN

    lookformax = True

    for i in arange(len(v)):
        this = v[i]
        if this > mx:
            mx = this
            mxpos = x[i]
        if this < mn:
            mn = this
            mnpos = x[i]

        if lookformax:
            if this < mx-delta:
                maxtab.append((mxpos, mx))
                mn = this
                mnpos = x[i]
                lookformax = False

        else:
            if this > mn+delta:
                mintab.append((mnpos, mn))
                mx = this
                mxpos = x[i]
                lookformax = True

    return array(maxtab)


def analyseSon():
    soundRecord("169.254.28.144", 9559)
    import os
    import pysftp

    curdir = os.getcwd()
    dir = curdir + '/audio/danseavec.wav'
    with pysftp.Connection('169.254.28.144', username = 'nao', password = 'stark') as sftp :
        sftp.get('/home/nao/danseavec.wav', dir)

    from numpy import size
    import scipy.io.wavfile as wave
    rate,data = wave.read('/home/eleve/Bureau/audio/danseavec.wav')
    #print rate
    #print data
    if len(data) != size(data) :
        data=data[:,0]

    import numpy
    Debut = 10
    duree = 5
    N = duree*rate
    f = data[ Debut : N + Debut]

    axex = numpy.arange(size(f))/rate

    fvi = numpy.fft.fft(f, n=2*N)
    acf = numpy.real( numpy.fft.ifft( fvi * numpy.conjugate(fvi) )[:N] )
    acf = acf/N


    acfcarre=acf*acf


    moy = 2000

    rate1 = rate / moy
    ajf = []
    count = 0
    ajfa = 0
    while size(ajf) != size (acfcarre) / moy :
        for it in range (moy) :
            ajfa = acfcarre[it+count*moy] + ajfa
        ajf = ajf + [ajfa]

        ajfa = 0
        count = count + 1


    ajf=ajf/ajf[0] * 100
    axex = numpy.arange(size(ajf))/rate1
    Npic=len(peakdet(ajf, 0.1, axex))
    BPM = Npic * 60 / duree
    return BPM



while True :
    from naoqi import ALProxy
    import sys


    port = 9559
    if len(sys.argv) < 2:
        print("veuillez passer une ip en cli")
        sys.exit(1)

    ip = sys.argv[1]
    postureProxy = ALProxy("ALRobotPosture", ip, 9559)
    postureProxy.goToPosture("Stand", 0.5)
    motionProxy = ALProxy ("ALMotion", ip, 9559)


    def BPMs():

        BPM= analyseSon()
        if 60<=BPM<=70:
            a = 1.5
        elif 70<=BPM<=80:
            a = 1.3
        elif 80<=BPM<=90:
            a = 1.1
        elif 90<=BPM<=100:
            a = 0.9
        elif 100<=BPM<=110:
            a = 0.7
        elif 110<=BPM<=120:
            a = 0.5
        elif 120<=BPM<=140:
            a = 0.4
        print BPM
        return a

    x = BPMs()

          ##COUDES EN L'AIR GAUCHE
    names = ['RElbowRoll' ,'RShoulderRoll','LShoulderRoll','LElbowRoll','RShoulderPitch','LShoulderPitch','RElbowYaw','LElbowYaw', 'HeadYaw', 'HeadPitch']
    times = [[x],[x],[x],[x],[x],[x],[x],[x],[x],[x]]

    for i in range (4):
        motionProxy.angleInterpolation(names,[1.15,-1.13,1.17,-1.18,1.6,1.6,0.13,-0.13,1.45,-0.28],times, True)
        motionProxy.angleInterpolation(names,[0.045,-0.045,-0.06,-0.04537,1.6,1.6,0.13,-0.13,1.45,0.21],times, True)



        ##COUDES EN L'AIR DROITE


    names = ['RElbowRoll' ,'RShoulderRoll','LShoulderRoll','LElbowRoll','RShoulderPitch','LShoulderPitch','RElbowYaw',     'LElbowYaw', 'HeadYaw', 'HeadPitch']
    times = [[x],[x],[x],[x],[x],[x],[x],[x],[x],[x]]

    for i in range (4):
        motionProxy.angleInterpolation(names,[1.15,-1.13,1.17,-1.18,1.6,1.6,0.13,-0.13,-1.45,-0.28],times, True)
        motionProxy.angleInterpolation(names,[0.045,-0.045,-0.06,-0.04537,1.6,1.6,0.13,-0.13,-1.45,0.21],times, True)


            ##DAB
    x = BPMs()
    names = ['RElbowRoll' ,'RShoulderRoll','RShoulderPitch','RElbowYaw','LShoulderPitch','LShoulderRoll','LElbowYaw','HeadPitch','HeadYaw','LHipPitch','RHipPitch','LKneePitch','RKneePitch','LAnklePitch','RAnklePitch']
    times = [[2*x],[2*x],[2*x],[2*x],[2*x],[2*x],[2*x],[2*x],[2*x],[2*x],[2*x],[2*x],[2*x],[2*x],[2*x]]

    for i in range (2):
        motionProxy.angleInterpolation(names,[0.035,-0.28,0.91,-0.4,1.2,0,-0.17,-0.21,0,0,0,0,0,0,0],times, True)
        motionProxy.angleInterpolation(names,[1.38,0.28,0.4,0.47,1.11,1.3,2.08,0.51,-0.4,-0.994,-0.994,1.24,1.24,-0.49,-0.49],times, True)
        motionProxy.angleInterpolation(names,[0.035,-0.28,0.91,-0.4,1.2,0,-0.17,-0.21,0,0,0,0,0,0,0],times, True)

            ##COUCOU

    x = BPMs()
    names = ['RElbowRoll' ,'RShoulderRoll','LShoulderRoll','LElbowRoll','RShoulderPitch','LShoulderPitch','RElbowYaw', 'LElbowYaw',]
    times = [[x],[x],[x],[x],[x],[x],[x],[x]]

    for i in range (8):
        motionProxy.angleInterpolation(names,[0.98,-0.07,1.1,-0.05,-1.46,-1.6,0,0],times, True)
        motionProxy.angleInterpolation(names,[0.034,-1.06,-0.07,-1,-1.6,-1.6,0,0],times, True)


            ##GENOU
    x = BPMs()
    names = ['RElbowRoll' ,'RShoulderRoll','RShoulderPitch','RElbowYaw','LElbowRoll','LShoulderRoll','LShoulderPitch','LElbowYaw','RAnklePitch','LAnklePitch', 'LKneePitch', 'RKneePitch','LHipPitch','RHipPitch']
    times = [[x],[x],[x],[x],[x],[x],[x],[x],[x],[x],[x],[x],[x],[x]]

    for i in range (4):
        motionProxy.angleInterpolation(names,[1.45,0.23,0,0,-1.33,-0.07,0.87,-0.2,0.09,0.09,-0.09,-0.09,0.13,0.13],times, True)
        motionProxy.angleInterpolation(names,[1.45,0.23,0,0,-1.33,-0.07,0.87,-0.2,-0.63,-0.63,1.3,1.3,-0.43,-0.43],times, True)
        motionProxy.angleInterpolation(names,[1.45,0.23,0,0,-1.33,-0.07,0.87,-0.2,0.09,0.09,-0.09,-0.09,0.13,0.13],times, True)

            ##petits mouvements bras

    x = BPMs()

    names = ['RElbowRoll' ,'LElbowRoll', 'RShoulderRoll','LShoulderRoll','RShoulderPitch','LShoulderPitch','HeadYaw']
    times = [[x],[x],[x],[x],[x],[x],[x]]

    for i in range (8):
        motionProxy.angleInterpolation(names,[0.65,-0.65,0.14,-0.14,0.3,0.71,-0.46],times, True)
        motionProxy.angleInterpolation(names,[0.65,-0.65,0.14,-0.14,0.71,0.3,0.46],times, True)

            ##BRAS EN L'AIR
    x = BPMs()

    names = ['RElbowRoll' ,'LElbowRoll', 'RShoulderRoll','LShoulderRoll','RShoulderPitch','LShoulderPitch','HeadPitch']
    times = [[2*x],[2*x],[2*x],[2*x],[2*x],[2*x],[2*x]]

    for i in range (4):
        motionProxy.angleInterpolation(names,[0.06,-0.16,0.29,-0.28,0.84,0.84,0.36],times, True)
        motionProxy.angleInterpolation(names,[-0.07,0.04,0.17,-0.14,-1.5,-1.5,-0.44],times, True)
        motionProxy.angleInterpolation(names,[0.06,-0.16,0.29,-0.28,0.84,0.84,0.36],times, True)



    time.sleep(1)