#!/usr/bin/python
# -*- coding: utf-8 -*-

"""
Program rdanse

author : Thierry Dassé
version : 0.1.0
date : 08/19/2016
licence : cc-by-sa-nc
"""

import pypot.dynamixel
import time,json

class Robot(object):
    """
    Robot class
    provides functions to make moves on a dynamixel servomotors robot
    """
    def __init__(self,base='',dico=''):
        """
        init the Robot class
        base is movement position base
        dico is an alias dictionary
        """
        self.scan = {} #robot config {'usb port':{'io':dxl-io,'idm':motor_id}}
        if base:
            self.pos = self.read(base) #learn base {'letter':{time:{idm:relative_angle}}}
        else:
            self.pos = {} 
        if dico:
            self.dico = self.read(dico) #dico {'name': 'sequence'}
        else:
            self.dico = {}

    def connect(self,max = 60):
        """
        search motors ids on usb port and connect to the robot
        return the number of motors found 
        """
        self.scan = {} #robot config
        idn = 0 #motor number
        ports = pypot.dynamixel.get_available_ports()
        for port in ports:
            dxl_io = pypot.dynamixel.DxlIO(port)
            id_moteur = dxl_io.scan(range(max))
            self.scan[port] = {'io':dxl_io,'idm':id_moteur}
            idn += len(id_moteur)
        return idn

    def get_io(self,id_moteur):
        """
        return io of a motor id
        """
        for port in self.scan:
            if id_moteur in self.scan[port]['idm']:
                return self.scan[port][dxl_io]
        raise ValueError('{} nof found'.format(id_moteur))
            
    def get_all_id(self):
        """
        return all robot ids
        """
        idm = []
        for port in self.scan:
            idm = idm + self.scan[port]['idm']
        return idm            

    def enable(self,ids = 'all'):
        """
        set enable_torque to motors
        ids can be 'all' (default) or a list of motors 
        """
        if ids == 'all':
            for port in self.scan:
                self.scan[port]['io'].enable_torque(self.scan[port]['idm'])
        else:
            for port in self.scan:
                for i in ids:
                    if i in self.scan[port]['idm']:
                        self.scan[port]['io'].enable_torque([i])

    def disable(self,ids = 'all'):
        """
        set disable_torque to motors
        ids can be 'all' (default) or a list of motors 
        """
       if ids == 'all':
            for port in self.scan:
                self.scan[port]['io'].disable_torque(self.scan[port]['idm'])
        else:
            for port in self.scan:
                for i in ids:
                    if i in self.scan[port]['idm']:
                        self.scan[port]['io'].disable_torque([i])

    def set_speed(self,dic):
        """
        set moving_speed to motors
        dic is a id:speed dictionary
        """
        for port in self.scan:
            for key,value in dic.items():
                if key in self.scan[port]['idm']:
                    self.scan[port]['io'].set_moving_speed({key:value})
    
    def goto(self,dic):
        """
        move motors to an angular position
        dic is a id:angle dictionary
        """
        for port in self.scan:
            self.scan[port]['goal'] = {}
        for key,value in dic.items():
            for port in self.scan:
                if key in self.scan[port]['idm']:
                    self.scan[port]['goal'][key] = value

        for port in self.scan:
            self.scan[port]['io'].set_goal_position(self.scan[port]['goal'])

    def get(self,ids = 'all'):
        """
        return a id:angle dictionary
        ids can be 'all' (default) or a list of motors 
        """
        pos = {}
        for port in self.scan:
            result = self.scan[port]['io'].get_present_position(self.scan[port]['idm'])
            for i in range(len(result)):
                pos[self.scan[port]['idm'][i]] = result[i]
        if ids == 'all':
            return pos
        else:
            pos_ids = {}
            for i in ids:
                pos_ids[i] = pos[i]
            return pos_ids

    def learn_home(self,name,ids='all'):
        """
        start to learn robot moves
        name is the name in the learning base
        ids can be 'all' (default) or a list of motors
        motors in ids list are enabled not to move
        other motors are free to be manually moved
        
        store the position to be a home in the relative movement
        """
        self.enable()
        self.home_pos = self.get(ids)
        self.home_name = name
        self.disable(ids)
        self.home_ids = ids

    def diff(self):
        """
        return the différence between home and current position for motors in home ids
        """
        p = self.get(self.home_ids)
        r = {}
        for key,value in p.items():
            r[key] = value - self.home_pos[key]
        return r

    def add(self,pos):
        """
        return the sum between home and current position for motors in home ids
        """
        r = {}
        for key,value in pos.items():
            r[key] = value + self.home_pos[key]
        return r

    def learn(self,time):
        """
        learn the difference between current and home position at time date
        """
        try:
            self.pos[self.home_name]
        except:
            self.pos[self.home_name] = {}
        self.pos[self.home_name][time] = self.diff()

    def play(self,word,return_to_zero=True,absolute=False):
        """
        play the word
        if return_to_zero is True (default) return to the 'zero' position at the end
        'zero' must be defined in the learning base
        if adsolute is True (default is False), first position of moves will be considered as absolute angles values and not relative from the current position
        word syntax is a list of defined moves separated by - and +
        moves separated by - will be played simultaneously
        moves separated by + will be played consecutive

        example:
        with 'r-ou+g-e' 'r' and 'ou' will be played together and after 'g' and 'e' will be played together two
        """
        syllab = word.split('+')
        first = True
        for s in syllab:
            letters = s.split('-')
            duration = 0
            dkeys = []
            for l in letters:
                duration =max(duration,max(self.pos[l].keys()))
                dkeys += self.pos[l].keys()
            dkeys = list(set(dkeys))
            dkeys.sort()
##            print '*dkey**** ',dkeys
            idd = {}
            for l in letters:
                idd[l]={}
                for t in self.pos[l].keys():
                    for idm,angle in self.pos[l][t].items():
                        try:
                            idd[l][idm][t] = angle
                        except:
                            idd[l][idm] = {}
                            idd[l][idm][t] = angle
##            print '*idd***** ',idd
            for l in letters:
                for idm in idd[l].keys():
                    for d in dkeys:
                        try:
                            idd[l][idm][d]
                        except:
                            dp = idd[l][idm].keys()
                            dp.sort()
                            if d < dp[0]:
                                idd[l][idm][d] = idd[l][idm][dp[0]] * d / dp[0]
                            elif d > dp[-1]:
                                idd[l][idm][d] =  idd[l][idm][dp[-1]]
                            else:
                                i = 0
                                while d  > dp[i]:
                                    i += 1
                                idd[l][idm][d] = idd[l][idm][dp[i-1]] + (d - dp[i-1]) * (idd[l][idm][dp[i]] - idd[l][idm][dp[i-1]])/float((dp[i] - dp[i-1]))
##            print '*idd***** ',idd
            pos = {}
            for d in dkeys:
                pos[d] = {}

            for l in letters:
                for idm in idd[l].keys():
                    for d,angle in idd[l][idm].items():
                        try:
                            pos[d][idm] += angle
                        except:
                            pos[d][idm] = angle
            t = 10
            i = 0
            self.home_pos = self.get()
            if first and absolute:
                tmin = min(pos.keys())
                for idm,val in self.home_pos.items():
                    try:
                        pos[tmin][idm] -= val
                    except:
                        pos[tmin][idm] = -val
            first = False

            speed = {}
            for idm,value in pos[dkeys[0]].items():
                speed[idm] = abs(value) * 100 / dkeys[i]
            self.set_speed(speed)
            
            while t <= duration:
                if t > dkeys[i]:
                    while t > dkeys[i]:
                        i += 1
                        speed = {}    
                        for idm,value in pos[dkeys[i]].items():
                            speed[idm] = abs(value - pos[dkeys[i-1]][idm]) * 100 / (dkeys[i] - dkeys[i-1])
                        self.set_speed(speed)
                if i == 0:
                    inter = {}
                    for idm,value in pos[dkeys[i]].items():
                        inter[idm] = self.home_pos[idm] + value * float(t) / dkeys[i]
                else:
                    inter = {}
                    for idm,value in pos[dkeys[i]].items():
                        inter[idm] = self.home_pos[idm] + pos[dkeys[i-1]][idm] + (value - pos[dkeys[i-1]][idm])* (float(t) - dkeys[i-1]) / (dkeys[i] - dkeys[i-1])
                self.goto(inter)
                time.sleep(0.1)
                t+= 10

        if return_to_zero:
            self.play('zero',False,True)

    def playw(self,word,return_to_zero=True,absolute=False):
        """
        same as play but search the word in the alias dictionary where you can define
        'r-ou+g-e' as 'rouge' for example
        """
        self.play(self.dico[word],return_to_zero,absolute)

    def acode(self,data):
        if type(data) == unicode:
            d =  data.encode('ascii')
            try :
                return int(d)
            except:
                return d
        else:
            return data

    def acode_dict(self,data):
        return dict(map(self.acode, pair) for pair in data.items())

    def read(self,filename):
        f = open(filename)
        dic = json.load(f,object_hook=self.acode_dict)
        f.close()
        return dic

    def load_pos(self,filename):
        """
        save a learning base
        """
        self.pos = self.read(filename)

    def save_pos(self,filename):
        """
        save the current learning base
        """
        f = open(filename, 'w')
        json.dump(self.pos, f, indent=4)
        f.close()

    def load_dict(self,filename):
        """
        load a word alias dictionary
        """
        self.dico = self.read(filename)

              
##r = Robot('base.json','dico.json')
##a = r.connect()
##print('connexion : {} moteurs trouvés'.format(a))