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,
        'turbine': 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]
        },
        'turbine': {
            'slot': 7,
            'range':[307,410],
            'stop': 0
         }
    }
    escSlotsList= [12,13,8,9]
    def set_range(self, name:str, mini:int, maxi:int):
        self.escMapping[name]['range'] = [mini,maxi]
    def init(self):
        print("drive init start")
        v = self.escMapping['turbine']
        for i in range(220, 250, 1):
            drive.setPwm(v['slot'], 0, i)
            sleep(0.04)
        drive.setPwm(v['slot'], 0, 0)
        print("drive init stop")

    #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'])
                )

    def set_raw_esc(self, esc_name, value):
        drive.setPwm(
            self.escMapping[esc_name]['slot'],
            0,
            value
        )
    
    # é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)

    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):
        self.setSpeed({
                      'frontLeft': speed,
                      'frontRight':speed,
                      'backLeft': speed,
                      'backRight': speed
                  })

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

    def northEastTranslation(self, speed):
        a = speed
        s = 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 setTurbine(self, speed):
        self.setSpeed({
            'turbine':0
            })

    def stop(self):
        if drive != None:
            for k,v in self.escMapping.items():
                if k == 'turbine':
                    continue
                drive.setPwm(v['slot'], 0, v['range'][0])
        """
        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
                      })