import ast

import kociemba
import requests
import packages.move as move


class machine:
    # trying to make this class support direct control mode first (i.e. from an external computer)
    # And then implement a second thing which runs off of the moveList class's 'generate_list_request' to send
    # all the commands to the webserver at once.

    url = None
    overshoot_counters = [0, 0, 0, 0]
    last_used_counters = [0, 0, 0, 0, 0, 0, 0, 0]
    tick_length = 0.05
    servoAngles = [0 for _ in range(9)]  # the actual angle of the servos
    # initialing both of these position lists with values representing 'unknown'
    rotatorPositions = [-1, -1, -1, -1]  #
    retractorPositions = ['n', 'n', 'n', 'n']

    overshoot_angles_cube = {  # how much to 'overshoot' when performing rotations.
        # There's overrides for all 4 positions and their 2 respective directions
        # obviously, the two endpoints only have a single direction
        # todo implement some simple stuff for the two levels of wobble - i.e on the cube itself,
        #  and within the machine as well
    }
    overshoot_angles_machine = {  # how much to overshoot again when performing rotations not
        # involving interfacing with the cube.
        # These only have one value because it's mainly a visual thing, and it makes calibrating easier
        # they also apply to the retractor servos as well
    }
    rotator_servo_angles = {}
    retraction_servo_angles = {}
    overshootWait = [0, 0.3, 0.4, 0.5]  # how long to wait after each length of move before turning the servo back
    lastUsedWait = 1

    def __init__(self, _url):
        self.url = _url
        self.read_calibration_file()

    def read_calibration_file(self):
        with open('packages/calibration.txt') as calibration_file:
            calibration_vars = ast.literal_eval(calibration_file.read())
            self.overshoot_angles_machine = calibration_vars['overshoot_angles_machine']
            self.overshoot_angles_cube = calibration_vars['overshoot_angles_cube']
            self.rotator_servo_angles = calibration_vars['rotator_servo_angles']
            self.retraction_servo_angles = calibration_vars['retraction_servo_angles']

    def apply_machine_overshoots(self, index, angle):
        if self.servoAngles[index] > angle:
            angle -= self.overshoot_angles_machine[index]
        elif self.servoAngles[index] < angle:
            angle += self.overshoot_angles_machine[index]
        return angle

    def apply_cube_overshoots(self, index, position):
        last_position = self.rotatorPositions[index]
        if last_position > position:

            new_angle = (self.rotator_servo_angles[index][position]
                         + self.overshoot_angles_cube[index][position][1])
        elif last_position < position:
            new_angle = (self.rotator_servo_angles[index][position]
                         - self.overshoot_angles_cube[index][position][0])
        else:
            new_angle = last_position
        return new_angle

    def tick(self):
        # handles timings of stuff like overshoot moves and turning off the servos if they haven't been used for a while
        for i, counter in enumerate(self.overshoot_counters):
            if counter > 0:
                self.overshoot_counters[i] -= self.tick_length
                if self.overshoot_counters[i] <= 0:
                    # set the servo's angle to it's non-overshot angle once the counter has finished.
                    self.set_angle(i + 4, self.rotator_servo_angles[i][self.rotatorPositions[i]])
        for i, counter in enumerate(self.last_used_counters):
            if counter < 1:
                self.last_used_counters[i] += self.tick_length
                if self.last_used_counters[i] >= self.lastUsedWait:
                    # set the servo's angle to it's non-overshot angle once the counter has finished.
                    self.stop(i)
        self.read_calibration_file()

    def set_angle(self, index, angle):
        # the index sent to this class is the true index of the servo =
        # i.e on the physical servo controller
        self.last_used_counters[index] = 0
        requests.post(self.url + 'receiveDirect', data={
            "component": "servo",
            "index": index,
            "value": self.apply_machine_overshoots(index, angle),
        })
        self.servoAngles[index] = angle

    def stop(self, index):
        requests.post(self.url + 'receiveDirect', data={
            "component": "servo",
            "index": index,
            "value": -180,
        })

    def set_leds(self, index, colour):
        requests.post(self.url + 'receiveDirect', data={
            "component": "led",
            "index": index,
            "value": colour,
        })

    def take_photo(self):
        requests.post(self.url + 'receiveDirect', data={
            "component": "camera",
        })

    def get_colours(self):
        response = requests.get(self.url + 'get_data', data={
            "request": "image"
        })
        return ast.literal_eval(response.content.decode("utf+8"))

    def goto(self, index, position):
        new_angle = self.apply_cube_overshoots(index, position)
        time_offset = 0
        if self.rotatorPositions[index] != -1:  # don't set the time offset if we don't know the last position
            time_offset = self.overshootWait[abs(self.rotatorPositions[index] - position)]

        if time_offset != 0:  # don't bother with overshoot angles if the time offset is zero
            # set the position
            self.set_angle(index + 4, new_angle)
            self.overshoot_counters[index] = time_offset
        else:
            self.set_angle(index + 4, self.rotator_servo_angles[index][position])
        self.rotatorPositions[index] = position

    def move(self, index, move_by):
        move_to = (self.rotatorPositions[index] + move_by) % 4
        self.goto(index, move_to)

    def retract(self, index):
        self.retractorPositions[index] = 'r'
        self.set_angle(index, self.retraction_servo_angles['retract'][index])

    def extend(self, index):
        self.retractorPositions[index] = 'e'
        self.set_angle(index, self.retraction_servo_angles['extend'][index])

    def full_extend(self, index):
        self.retractorPositions[index] = 'f'
        self.set_angle(index, self.retraction_servo_angles['fullExtend'][index])

    def solve_cube(self, cube_string):
        solution = kociemba.solve(cube_string)
        face_servo_dict = {
            'B': 0,
            'R': 1,
            'F': 2,
            'L': 3,
            'U': 4,
            'D': 5,
        }
        solution = solution.split()

        move_list = move.MoveList(self, [])
        for i in solution:
            servo = face_servo_dict[i[0]]
            if servo > 3:
                face_servo_dict = self.add_rotate_move(move_list, face_servo_dict)
                servo = face_servo_dict[i[0]]
            if i[-1] == "'":
                move_list.add_move('spin', servo, (self.rotatorPositions[servo] + 1) % 4)
                self.rotatorPositions[servo] = (self.rotatorPositions[servo] + 1) % 4
            elif i[-1] == "2":
                move_list.add_move('spin', servo, (self.rotatorPositions[servo] + 2) % 4)
                self.rotatorPositions[servo] = (self.rotatorPositions[servo] + 2) % 4
            else:
                move_list.add_move('spin', servo, (self.rotatorPositions[servo] - 1) % 4)
                self.rotatorPositions[servo] = (self.rotatorPositions[servo] - 1) % 4
            move_list.add_move('delay',_delay=1)

        print('e')
        move_list.send_list_request()

    def add_rotate_move(self, move_list, face_servo_dict):
        # check if clockwise 13 rotation is valid
        if self.rotatorPositions[1] != 0 and self.rotatorPositions[3] != 3:
            move_list.add_move('extension', 0, 'r')
            move_list.add_move('extension', 2, 'r')
            move_list.add_move('delay', _delay=.5)
            move_list.add_move('spin', 1, self.rotatorPositions[1] - 1)
            move_list.add_move('spin', 3, self.rotatorPositions[3] + 1)
            move_list.add_move('spin', 0, 1)
            move_list.add_move('spin', 2, 1)
            move_list.add_move('delay', _delay=.5)
            move_list.add_move('extension', 0, 'e')
            move_list.add_move('extension', 2, 'e')
            self.rotatorPositions[1] -= 1
            self.rotatorPositions[3] += 1
            self.rotatorPositions[0] = 1
            self.rotatorPositions[2] = 1
        # # else try an anticlockwise 13 rotation
        # elif self.rotatorPositions[1] != 3 and self.rotatorPositions[3] != 0:
        #
        # # else try a clockwise 02 rotation
        # elif self.rotatorPositions[0] != 0 and self.rotatorPositions[2] != 3:
        # # else try an anticlockwise 02 rotation
        # elif self.rotatorPositions[0] != 3 and self.rotatorPositions[2] != 0:
        # # else retract 13 and set them to a position where they can perform a clockwise rotation
        else:
            move_list.add_move('extension', 1, 'r')
            move_list.add_move('extension', 3, 'r')
            move_list.add_move('delay', _delay=.5)
            move_list.add_move('spin', 1, 2)
            move_list.add_move('spin', 3, 0)
            move_list.add_move('delay', _delay=.5)
            move_list.add_move('extension', 1, 'e')
            move_list.add_move('extension', 3, 'e')
            move_list.add_move('delay', _delay=.5)
            move_list.add_move('extension', 0, 'r')
            move_list.add_move('extension', 2, 'r')
            move_list.add_move('delay', _delay=.5)

            move_list.add_move('spin', 1, 1)
            move_list.add_move('spin', 3, 1)
            move_list.add_move('spin', 0, 1)
            move_list.add_move('spin', 2, 1)
            move_list.add_move('delay', _delay=.5)
            move_list.add_move('extension', 0, 'e')
            move_list.add_move('extension', 2, 'e')
            self.rotatorPositions[1] = 1
            self.rotatorPositions[3] = 1
            self.rotatorPositions[0] = 1
            self.rotatorPositions[2] = 1

        k = list(face_servo_dict.keys())
        k = [
            k[5],
            k[1],
            k[4],
            k[3],
            k[0],
            k[2],
        ]
        v = list(face_servo_dict.values())
        face_servo_dict = {k[i]: v[i] for i in range(6)}
        return face_servo_dict

    def execute_move_list(self, move_list):
        raw_move_list = []
        time = 0
        # move lists use an absolute time to execute moves instead of waiting between moves
        for move in move_list:  # convert each move in move_list to a request
            match move.moveType:
                case 'spin':
                    new_angle = self.apply_cube_overshoots(move.index, move.position)

                    time_offset = 0
                    if self.rotatorPositions[move.index] != -1:
                        # don't set the time offset if we don't know the last position
                        time_offset = self.overshootWait[abs(self.rotatorPositions[move.index] - move.position)]

                    # if time_offset != 0:  # don't bother with overshoot angles if the time offset is zero
                    #     raw_move_list.append({
                    #         'component': 'servo',
                    #         'index': move.index + 4,
                    #         'value': new_angle
                    #         'value': self.apply_machine_overshoots(move.index + 4, new_angle),
                    #         'time': time,
                    #     })

                    raw_move_list.append({
                        'component': 'servo',
                        'index': move.index + 4,
                        'value':
                            self.apply_machine_overshoots(move.index + 4,
                                                          self.rotator_servo_angles[move.index][move.position]),
                        'time': time# + time_offset
                    })
                    print(new_angle)
                    print(time_offset)
                    print(self.rotator_servo_angles[move.index][move.position])
                    # update internal stored positions
                    self.servoAngles[move.index + 4] = self.rotator_servo_angles[move.index][move.position]
                    self.rotatorPositions[move.index] = move.position
                    print(self.rotatorPositions)
                case 'extension':
                    # for converting between some of the random variable names
                    # todo obsolete this
                    converter_dict = {'r': 'retract', 'e': 'extend', 'f': 'fullExtend'}
                    value = self.retraction_servo_angles[converter_dict[move.position]][move.index]
                    raw_move_list.append({
                        'component': 'servo',
                        'index': move.index,
                        'value': value,
                        'time': time
                    })

                    # update internal stored positions
                    self.retractorPositions[move.index] = move.position
                case 'delay':
                    time += move.delay
                case 'stop':
                    raw_move_list.append({
                        'component': 'servo',
                        'index': move.index,
                        'value': -180,
                        'time': time
                    })
                    raw_move_list.append({
                        'component': 'servo',
                        'index': move.index + 4,
                        'value': -180,
                        'time': time
                    })
                case 'led':
                    raw_move_list.append({
                        'component': 'led',
                        'index': move.index,
                        'value': move.colour,
                        'time': time
                    })
                case 'photo':
                    raw_move_list.append({
                        'component': 'camera',
                        'time': time
                    })
        # add in a stop message to the end of the list
        for i in range(9):
            raw_move_list.append({
                'component': 'servo',
                'index': i,
                'value': -180,
                'time': time + 1
            })
        requests.post(self.url + 'receiveList', json=raw_move_list)


class Counter:
    servo = None
    angle = None
    delay = None

    def __init__(self, n_servo, n_angle, n_delay):
        self.servo = n_servo
        self.angle = n_angle
        self.delay = n_delay
