import os
import time
import poppytools

from pypot.robot import from_json

config_path = os.path.join(os.path.dirname(poppytools.__file__), 'configuration', 'poppy_config.json')

poppy = from_json(config_path)
poppy.start_sync()

import json

def save_pos(filename, pos):
    with open(filename, 'w') as f:
        json.dump(pos, f, indent=2)
        
def load_pos(filename):
    with open(filename) as f:
        return json.load(f)
    
def get_pos(motors):
    return {m.name: m.present_position for m in motors}

def set_pos(pos):
    for n, p in pos.items():
        getattr(poppy, n).goal_position = p

from pyo import *

s = Server().boot()
s.start()

bob_sf = [
    SfPlayer('sounds/kicks/kick08_good.wav', mul=0.8).out(),
    SfPlayer('sounds/snare05.wav', mul=0.8).out(),
    SfPlayer('sounds/chorus03.wav', mul=0.8).out(),
    SfPlayer('sounds/chorus01.ogg', mul=0.8).out(),
]

class Sampler(object):
    def play(self, id):
        bob_sf[id].play().out()
        
sampler = Sampler()

s.recstart(filename='6.wav')

def play_measure(move):
    dur = 2.
    
    h1 = load_pos('pos/head_1.json')
    h2 = load_pos('pos/head_2.json')
    lk1 = load_pos('pos/left_kick_1.json')
    lk2 = load_pos('pos/left_kick_2.json')
    
    tete = False
    j = 0
    
    while True:
        if j > 3:
            tete = True
            
        for i in range(8):
            if i in (2, 6) and tete:
                sampler.play(1)
                if move:
                    set_pos(h1)
                
            if i in (3, 7) and tete:
                if move:
                    set_pos(h2)
                
            if i in (0, 5):
                #sampler.play(0)
                if move:
                    set_pos(lk1)
                
            if i in (1, 6):
                if move:
                    set_pos(lk2)
        
            time.sleep(0.2)
            
        j += 1

sf = SfPlayer('sounds/MachineDrumSquareC.wav', loop=True)
filter = Biquad(sf, freq = 100)
effect = Chorus(filter)
effect.out()

sf.setMul(0.75)

from pypot.primitive import LoopPrimitive

class LiveMusic(LoopPrimitive):
    def update(self):
        x = poppy.l_shoulder_x.present_position / 90.
        filter.setFreq(10 + x * 2500)
        
        y = -poppy.l_shoulder_y.present_position - 50
        y = min(max(y / 100., 0), 1)
        sf.setSpeed(0.2 + 3.8 * y)
        
live = LiveMusic(poppy, 50)

poppy.r_shoulder_x

from pypot.primitive import LoopPrimitive

class Blah(LoopPrimitive):
    def setup(self):
        self.play = False
        
    def update(self):
        if poppy.r_shoulder_y.present_position < -50 and not self.play:
            self.play = True
            
            x = poppy.r_shoulder_x.present_position + 25
            i = 2 if x > 0 else 3
            
            sampler.play(i)
            
        if poppy.r_shoulder_y.present_position > -40 and self.play:
            self.play = False
            
class Spoueh(LoopPrimitive):
    def setup(self):
        self.play = False
        
    def update(self):
        if poppy.l_ankle_y.present_position > -15 and not self.play:
            self.play = True
            sampler.play(0)
            
        if poppy.l_ankle_y.present_position < -20 and self.play:
            self.play = False
            
blah = Blah(poppy, 50)
blah.start()

spoueh = Spoueh(poppy, 50)
spoueh.start()

poppy.r_arm_z

live.start()
sf.play()

for m in [poppy.l_ankle_y,]:
    m.compliant = True

for m in [poppy.l_ankle_y, ]:
    m.compliant = False
    
play_measure(move=True)

sf.stop()
live.stop()

s.recstop()

SfPlayer('2eme_prise.wav', mul=0.8).out()