from time import sleep

'''
Abstration of motorized platform
'''
from PWMDriver import PWMDriver
drive = PWMDriver()

class MotorizedPlatform:
    esc_speed_map = {
        'frontLeft': 50,
        'frontRight': 50,
        'backLeft': 50,
        'backRight': 50
    }
    escMapping = {
        'frontLeft': {
            'slot': 12, # 7
            'range': [307, 410]
        },
        'frontRight': {
            'slot': 13,
            'range': [307, 410]
        },
        'backLeft': {
            'slot': 8,
            'range': [307, 410]
        },
        'backRight': {
            'slot': 9,
            'range': [307, 410]
        }
    }
    escSlotsList= [12,13,8,9]
    def set_range(self, name:str, mini:int, maxi:int):
        self.escMapping[name]['range'] = [mini,maxi]
    def init(self):
        if drive != None:
            for k,v in self.escMapping.items():
                # 307 est le signal neutre sous 50 Hz (1.5 / 20 x 4096 = 307)
                drive.setPwm(v['slot'], 0, 307)

    #escSlots = [7, 6, 8, 9]
    def setSpeed(self, values):
        # print('values: '+str(values))
        for k,v in values.items():
            self.set_esc_speed(k, v)

    def set_all(self, value):
        for k in self.escMapping.keys():
            self.set_esc_speed(k, value)

    def set_esc_speed(self, esc_name, value):
        if value < 0:
            value = value * 1.185
        drive.setPwm(
                self.escMapping[esc_name]['slot'],
                0,
                self.convert_speed_to_esc(value, self.escMapping[esc_name]['range'])
                )

        # équivalent de la fonction map() de arduino
    def mappyt(self, x, inMin, inMax, outMin, outMax):
        return (x - inMin) * (outMax - outMin) / (inMax - inMin) + outMin

    # fonction esc pour une vitesse de moteur de -100 à 100()
    def convert_speed_to_esc(self, speed, pwm_range=[307, 410]):
        return round(
            self.mappyt(speed, 0, 100, pwm_range[0], pwm_range[1])
        )

    def eastTranslation(self, speed):
        self.setSpeed({
                          'frontLeft': -speed, 'frontRight': -speed,
                          'backLeft':  speed,  'backRight': speed
                      })

    def southTranslation(self, speed):
        self.northTranslation(-speed*1.1)

    def northTranslation(self, speed):
        self.setSpeed({
                          'frontLeft': -speed, 'frontRight': speed,
                          'backLeft': -speed, 'backRight':  speed
                      })

    def westTranslation(self, speed):
        self.eastTranslation(- speed)

    def clockwiseRotation(self, speed):
        a = self.convert_speed_to_esc(speed)
        r = self.convert_speed_to_esc(-speed)
        self.setSpeed({
                      'frontLeft': a,
                      'frontRight': a,
                      'backLeft': r,
                      'backRight': r
                  })

    def antiClockwiseRotation(self, speed):
        self.clockwiseRotation(-speed)

    def northEastTranslation(self, speed):
        a = self.convert_speed_to_esc(speed)
        s = self.convert_speed_to_esc(0)
        self.setSpeed({
                          'frontLeft': a,
                          'frontRight': s,
                          'backLeft': s,
                          'backRight': a
                      })

    def southWestTranslation(self, speed):
        self.northEastTranslation(-speed)

    def northWestTranslation(self, speed):
        self.setSpeed({
                          'frontLeft': 0,
                          'frontRight': speed,
                          'backLeft': 0,
                          'backRight': speed
                      })

    def southEastTranslation(self, speed):
        self.northWestTranslation(-speed)

    def stop(self):
        realescMapping = {
            'frontLeft': {
                'slot': 12, # 7
                'range': [307, 410]
            },
            'frontRight': {
                'slot': 13,
                'range': [307, 410]
            },
            'backLeft': {
                'slot': 8,
                'range': [307, 410]
            },
            'backRight': {
                'slot': 9,
                'range': [307, 410]
            }
        }
        motornamelist = ['frontLeft','frontRight','backLeft','backRight']
        for i in range(len(motornamelist)):
            esc_name = motornamelist[i]
            drive.setPwm(
                    realescMapping[esc_name]['slot'],
                    0,
                    self.convert_speed_to_esc(0, realescMapping[esc_name]['range'])
            )

    def oldstop(self):
        self.escMapping = {
            'frontLeft': {
                'slot': 12, # 7
                'range': [307, 410]
            },
            'frontRight': {
                'slot': 13,
                'range': [307, 410]
            },
            'backLeft': {
                'slot': 8,
                'range': [307, 410]
            },
            'backRight': {
                'slot': 9,
                'range': [307, 410]
            }
        }
        self.setSpeed({
                          'frontLeft': 0,
                          'frontRight': 0,
                          'backLeft': 0,
                          'backRight': 0
                      })