import copy

import requests
import threading
import pygame
import sys
# import twophase.solver as sv
# import kociemba
import pygame
import sys
import time
import sched


# simple class to improve readability of the counters within servos
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


class Move:
    moveType = None
    side = None
    _validSides = {'b': 0, 'r': 1, 'f': 2, 'l': 3, 0: 0, 1: 1, 2: 2, 3: 3}

    position = None
    _validAngles = {0: 0, 1: 1, 2: 2, 3: 3}
    _validPositions = {'f': 'f', 'e': 'e', 'r': 'r'}
    delay = None

    def __init__(self, _move_type, _side=None, _position=None, _delay=0.0):
        self.type = _move_type
        if _move_type == 'spin':
            self.moveType = _move_type
            try:
                self.side = self._validSides.get(_side)
            except KeyError:
                raise ValueError("Invalid Side Num")

            try:
                self.position = self._validAngles.get(_position)
                if self.position is None:
                    raise ValueError("But why")
            except KeyError:
                raise ValueError("Invalid Side Num")
        elif _move_type == 'extension':
            self.moveType = _move_type
            try:
                self.side = self._validSides.get(_side)
            except KeyError:
                raise ValueError("Invalid Side Num")
            # quickly sanity check the input value
            try:
                self.position = self._validPositions.get(_position)
            except KeyError:
                raise ValueError("Position must be either f, e or r")

        elif _move_type == 'delay':
            self.moveType = _move_type
            self.delay = _delay
        elif _move_type == 'stop':
            self.moveType = _move_type
            self.side = _side
        else:
            raise ValueError("Invalid move type")

    def execute(self, _machine):
        if self.moveType == 'spin':
            _machine.goto(self.side, self.position)
        elif self.moveType == 'extension':
            if self.position == 'e':
                _machine.extend(self.side)
            elif self.position == 'r':
                _machine.retract(self.side)
            elif self.position == 'f':
                _machine.full_extend(self.side)
        elif self.moveType == 'delay':
            return self.delay
        elif self.moveType == 'stop':
            _machine.set_angle(self.side, None)
            _machine.set_angle(self.side + 4, None)

        return 0


class MoveList:
    moves: list[Move] = []
    moves2: list[Move] == []
    counter = 1
    machine = None
    waitSeconds = 0
    _running = False

    def __init__(self, _machine, _moves: list[Move]):
        self.machine = _machine,
        self.moves = _moves

    def execute(self):
        self.moves2 = copy.copy(self.moves)
        self._running = True

    def tick(self):
        if self._running:
            self.waitSeconds -= .05
            if self.waitSeconds <= 0:
                while True:
                    try:
                        move_delay = self.moves2[0].execute(machine)
                    except IndexError:
                        self._running = False
                        self.counter -= 1
                        if self.counter != 0:
                            self.execute()
                        break
                    self.moves2.pop(0)
                    if move_delay > 0:
                        self.waitSeconds = move_delay
                        break


class Machine:
    counters: list[Counter] = []
    servoAngles = [0 for _ in range(9)]
    servoPositions = [1, 1, 1, 1]
    extraMoveDegrees = 5
    rotator_servo_angles = {
        0: [[172], [163], [159], [173]],
        1: [[117], [111], [105], [122]],
        2: [[62], [62], [53], [70]],
        3: [[17], [10], [3], [16]],
    }
    retraction_servo_angles = {
        'fullExtend': [150, 120, 120, 130],
        'extend': [130, 110, 110, 115],
        'retract': [73, 73, 73, 73],
    }
    # how long to wait after each length of move before turning the servo back
    moveTimings = [0.1, 0.2, 0.35]

    def tick(self):
        for counter in self.counters:
            if counter.delay > 0:
                counter.delay -= .05
                if counter.delay <= 0:
                    self.set_angle(counter.servo, counter.angle)

    def set_angle(self, index, angle, delay=0.0):
        if delay == 0:
            if angle is None:
                requests.post(url, data={
                    "component": "servo",
                    "index": index,
                    "value": -180,
                })
            else:
                self.servoAngles[index] = angle
                requests.post(url, data={
                    "component": "servo",
                    "index": index,
                    "value": angle,
                })

        else:
            self.counters.append(Counter(index, angle, delay))

    def goto(self, index, position):
        # check which way the servo is moving and add a bit to account for hysteresis
        angle = self.rotator_servo_angles[position][index][0]
        if angle > self.servoAngles[index + 4]:
            new_pos = angle + self.extraMoveDegrees
        elif angle < self.servoAngles[index + 4]:
            new_pos = angle - self.extraMoveDegrees
        else:
            new_pos = angle
        delay = (abs(angle - self.servoAngles[index + 4]) / 700)
        # set the position
        self.set_angle(index + 4, new_pos)
        # schedule a new move in 0.1s to move to the original position
        self.set_angle(index + 4, angle, delay=delay + .1)
        #
        # self.set_angle(index + 4, angle)

        # self.set_angle(index + 4, None, delay=delay+.3)

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

    def retract(self, index):
        self.set_angle(index, None, delay=0.5)
        self.set_angle(index, self.retraction_servo_angles['retract'][index])

    def extend(self, index):
        self.set_angle(index, None, delay=0.5)
        self.set_angle(index, self.retraction_servo_angles['extend'][index])

    def full_extend(self, index):
        self.set_angle(index, None, delay=0.5)
        self.set_angle(index, self.retraction_servo_angles['fullExtend'][index])


if 'jank_fest':
    class Cube:
        faceLocation = {
            "U": 4,
            "D": 5,
            "L": 1,
            "R": 3,
            "F": 0,
            "B": 2
        }

        def rotateFB(self, invert):
            values = list(self.faceLocation.values())
            keys = list(self.faceLocation.keys())
            # D -> L -> U -> R
            if invert:
                values = [values[3], values[2], values[1], values[0], values[4], values[5]]
            self.faceLocation = dict(zip(keys, values))
            print(self.faceLocation)

        def rotateRL(self, invert):
            values = list(self.faceLocation.values())
            keys = list(self.faceLocation.keys())
            # F -> D -> B -> U
            if invert:
                values = [values[5], values[4], values[2], values[3], values[0], values[1]]
            self.faceLocation = dict(zip(keys, values))
            print(self.faceLocation)


    # __sssssssssssssss__

    # translator dictionaries
    npTr = {
        "n": 1,
        "p": -1
    }
    pnTr = {
        "n": "p",
        "p": "n"
    }

if 'Setup Pygame':
    def draw_text(text, x, y, color):
        text_surface = font.render(text, True, color)
        screen.blit(text_surface, (x, y))


    # Function to check if a point is inside a rectangle

    def is_mouse_over(pos, rect):
        return rect.left < pos[0] < rect.right and rect.top < pos[1] < rect.bottom


    # Function to create a button

    def draw_button(rect, color, text):
        pygame.draw.rect(screen, color, rect)
        draw_text(text, rect.x + 10, rect.y + 10, BLACK)


    class Button:
        rect = pygame.Rect
        isPressed = False
        name = "1"

        def draw(self, screen):
            if self.isPressed:
                pygame.draw.rect(screen, (0, 255, 0), self.rect)
            else:
                pygame.draw.rect(screen, (0, 0, 0), self.rect)
            draw_text(self.name, self.rect.x + 10, self.rect.y + 10, (255, 255, 255))

# ___VARIABLES___
# timing stuff
scheduler = sched.scheduler(time.time, time.sleep)
scheduler.run()

# Network Stuff
pi_ip = '192.168.0.204'

url = "http://{0}:5000/receive".format(pi_ip)

# Pygame Stuff
# Define colors
WHITE = (255, 255, 255)
BLACK = (0, 0, 0)
GRAY = (150, 150, 150)
screen_width, screen_height = 400, 300

buttons = []
buttonPoss = [
    (130, 10, 50, 50),
    (250, 130, 50, 50),
    (130, 250, 50, 50),
    (10, 130, 50, 50),
    (130, 70, 50, 50),
    (190, 130, 50, 50),
    (130, 190, 50, 50),
    (70, 130, 50, 50),
]

# key grabbing
buttonPressed = 0
number_input = ""

# Misc
# Servos Class
machine = Machine()

# Initialize pygame
pygame.init()

# ___SETUP___
# Set up the screen
screen = pygame.display.set_mode((screen_width, screen_height))
pygame.display.set_caption("Simple GUI with Pygame")

# Create a font
font = pygame.font.SysFont(None, 40)

# Main function for GUI
if __name__ == "__main__":
    delaydelay = 0.06
    # setup gui
    for i, pos in enumerate(buttonPoss):
        buttons.append(Button())
        buttons[i].rect = pygame.Rect(pos)
        buttons[i].name = str(i)
    extensionDelay = .2
    delay2 = .2

    solution = ["Rp", "Bn", "Bn", "Up", "Fn", "Fn", "Dn", "Fn", "Dn", "Bp", "Un", "Rp", "Un", "Un", "Rn", "Rn", "Dp",
                "Fn", "Fn", "Un", "Un", "Fn", "Fn", "Rn", "Rn", "Un", "Un", "Rn", "Rn", "Fn", "Fn"]
    servoMoves = MoveList(machine, [])
    servoAngles = [1, -1, -1, 1]
    num = 0
    cube = Cube()
    for move in solution:
        faceNum = cube.faceLocation[move[0]]
        # first, check if the move is on a face which is already in place
        # if the face is not on the plane, perform a retraction
        if faceNum > 3:
            # todo: pick which way to do the retraction
            cube.rotateRL(True)
            # check if the servos to rotate are at the right angle
            # if they are not, we need to retract and correct.
            # this means we need to rotate the servo back
            num += 1
            # if either of the servos are wrong, retract them both and set them to position 1 and 2, respectively
            if servoAngles[1] == 3 or servoAngles[3] == 0:
                servoMoves.moves.append(Move('extension', 1, _position='r'))
                servoMoves.moves.append(Move('extension', 3, _position='r'))
                servoMoves.moves.append(Move('delay', _delay=delaydelay))
                servoMoves.moves.append(Move('spin', 1, _position=1))
                servoMoves.moves.append(Move('spin', 3, _position=2))
                servoMoves.moves.append(Move('delay', _delay=delaydelay))
                servoMoves.moves.append(Move('extension', 1, _position='e'))
                servoMoves.moves.append(Move('extension', 3, _position='e'))
                servoMoves.moves.append(Move('delay', _delay=delaydelay))
                servoAngles[0] = 1
                servoAngles[2] = 2
                servoAngles[1] = 1
                servoAngles[3] = 2

            # rotate the cube
            # servoMoves.moves.append("0r00002r00501p00003n01002e00000e0150")  # most of this timing seems to be perfect
            servoMoves.moves.append(Move('extension', 0, _position='r'))
            servoMoves.moves.append(Move('extension', 2, _position='r'))
            servoMoves.moves.append(Move('delay', _delay=delaydelay))
            servoMoves.moves.append(Move('spin', 1, _position=servoAngles[1]+1))
            servoMoves.moves.append(Move('spin', 3, _position=servoAngles[3]-1))
            servoMoves.moves.append(Move('spin', 0, _position=1))
            servoMoves.moves.append(Move('spin', 2, _position=2))
            servoMoves.moves.append(Move('delay', _delay=delaydelay))
            servoMoves.moves.append(Move('extension', 0, _position='e'))
            servoMoves.moves.append(Move('extension', 2, _position='e'))
            servoMoves.moves.append(Move('delay', _delay=delaydelay))
            servoAngles[1] += 1
            servoAngles[3] -= 1


        # next, check if the servo is at the correct angle:
        faceNum = cube.faceLocation[move[0]]
        if abs(servoAngles[faceNum] + npTr[move[1]]) > 1:
            # if the servo cannot perform the move, do a retraction and reset its rotation
            servoMoves.moves.append(Move('extension', faceNum, _position='r'))
            servoMoves.moves.append(Move('delay', _delay=delaydelay))
            servoMoves.moves.append(Move('spin', faceNum, _position=servoAngles[faceNum] - npTr[move[1]]))
            servoMoves.moves.append(Move('delay', _delay=delaydelay))
            servoMoves.moves.append(Move('extension', faceNum, _position='e'))
            servoMoves.moves.append(Move('delay', _delay=delaydelay))

            servoAngles[faceNum] -= npTr[move[1]]

        servoMoves.moves.append(Move('spin', faceNum, _position=servoAngles[faceNum] + npTr[move[1]]))
        servoMoves.moves.append(Move('delay', _delay=delaydelay))
        servoAngles[faceNum] += npTr[move[1]]
    print(servoMoves)
    print(servoAngles)
    # moves = MoveList(machine, [
    #     # retract opposite servos and set to positions
    #     Move('extension', 0, _position='r'),
    #     Move('extension', 2, _position='r'),
    #     Move('delay', _delay=.1),
    #     Move('spin', 0, _position=0),
    #     Move('spin', 2, _position=2),
    #     Move('delay', _delay=.5),
    #     Move('extension', 0, _position='e'),
    #     Move('extension', 2, _position='e'),
    #     Move('delay', _delay=extensionDelay),
    #     Move('extension', 1, _position='r'),
    #     Move('extension', 3, _position='r'),
    #     Move('spin', 0, _position=2),
    #     Move('spin', 2, _position=0),
    #     Move('spin', 1, _position=1),
    #     Move('spin', 3, _position=0),
    #     Move('delay', _delay=delay2),
    #     ##
    #     Move('extension', 1, _position='e'),
    #     Move('extension', 3, _position='e'),
    #     Move('delay', _delay=extensionDelay),
    #     Move('extension', 0, _position='r'),
    #     Move('extension', 2, _position='r'),
    #     Move('spin', 1, _position=0),
    #     Move('spin', 3, _position=1),
    #     Move('spin', 0, _position=1),
    #     Move('spin', 2, _position=0),
    #     Move('delay', _delay=delay2),
    #     ##
    #     Move('extension', 0, _position='e'),
    #     Move('extension', 2, _position='e'),
    #     Move('delay', _delay=extensionDelay),
    #     Move('extension', 1, _position='r'),
    #     Move('extension', 3, _position='r'),
    #     Move('spin', 1, _position=1),
    #     Move('spin', 3, _position=0),
    #     Move('spin', 0, _position=0),
    #     Move('spin', 2, _position=1),
    #     Move('delay', _delay=delay2),
    #     ##
    #     Move('extension', 1, _position='e'),
    #     Move('extension', 3, _position='e'),
    #     Move('delay', _delay=extensionDelay),
    #     Move('extension', 0, _position='r'),
    #     Move('extension', 2, _position='r'),
    #     Move('spin', 1, _position=0),
    #     Move('spin', 3, _position=1),
    #     Move('spin', 0, _position=1),
    #     Move('spin', 2, _position=0),
    #     Move('delay', _delay=delay2),
    #     ##
    #     Move('extension', 0, _position='e'),
    #     Move('extension', 2, _position='e'),
    #     Move('delay', _delay=extensionDelay),
    #     Move('extension', 1, _position='r'),
    #     Move('extension', 3, _position='r'),
    #     Move('spin', 1, _position=1),
    #     Move('spin', 3, _position=0),
    #     Move('spin', 0, _position=0),
    #     Move('spin', 2, _position=1),
    #     Move('delay', _delay=delay2),
    #     Move('extension', 1, _position='e'),
    #     Move('extension', 3, _position='e'),
    #     Move('delay', _delay=1),
    #     Move('stop', 0),
    #     Move('stop', 1),
    #     Move('stop', 2),
    #     Move('stop', 3),
    # ])
    moves1 = MoveList(machine, [
        Move('extension', 0, _position='r'),
        Move('extension', 2, _position='r'),
        Move('delay', _delay=.1),
        Move('spin', 0, _position=0),
        Move('spin', 2, _position=0),
        Move('delay', _delay=.5),
        Move('extension', 0, _position='e'),
        Move('extension', 2, _position='e'),
        Move('delay', _delay=extensionDelay),
        # Move('extension', 1, _position='r'),
        # Move('extension', 3, _position='r'),
        # Move('spin', 1, _position=0),
        # Move('spin', 3, _position=0),
        # Move('delay', _delay=extensionDelay),
        # Move('extension', 1, _position='e'),
        # Move('extension', 3, _position='e'),

    ])

    moves2 = MoveList(machine, [
        Move('spin', 0, _position=1),
        Move('delay', _delay=delaydelay),
        Move('spin', 1, _position=1),
        Move('delay', _delay=delaydelay),
        Move('spin', 2, _position=1),
        Move('delay', _delay=delaydelay),
        Move('spin', 3, _position=1),
        Move('delay', _delay=delaydelay),
        Move('spin', 0, _position=0),
        Move('delay', _delay=delaydelay),
        Move('spin', 1, _position=0),
        Move('delay', _delay=delaydelay),
        Move('spin', 2, _position=0),
        Move('delay', _delay=delaydelay),
        Move('spin', 3, _position=0),
        Move('delay', _delay=delaydelay),
    ])

    clock = pygame.time.Clock()
    moves1.execute()
    while True:
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                pygame.quit()
                sys.exit()

            elif event.type == pygame.KEYDOWN:
                if event.key == pygame.K_RETURN:
                    servoMoves.execute()
                    # try:
                    #     num = int(number_input)
                    #     moves2.counter = num
                    #     moves2.execute()
                    # except:
                    #     pass
                    # machine.set_angle(buttonPressed, number_input)
                    # # print("Entered number:", number_input)
                    # data_to_send = {
                    #     "component" : "servo",
                    #     "index": buttonPressed,
                    #     "value": number_input,
                    # }
                    number_input = ""
                    # response = requests.post(url, data=data_to_send)
                elif event.key == pygame.K_BACKSPACE:
                    number_input = number_input[:-1]
                elif event.key == pygame.K_UP:
                    buttonPressed = 2
                elif event.key == pygame.K_RIGHT:
                    buttonPressed = 3
                elif event.key == pygame.K_DOWN:
                    buttonPressed = 0
                elif event.key == pygame.K_LEFT:
                    buttonPressed = 1
                else:
                    number_input += event.unicode
                if buttonPressed < 4:
                    if event.key == pygame.K_r:
                        machine.retract(buttonPressed)
                    elif event.key == pygame.K_e:
                        machine.extend(buttonPressed)
                    elif event.key == pygame.K_t:
                        machine.full_extend(buttonPressed)
                    elif event.key == pygame.K_a:
                        machine.goto(buttonPressed, 0)
                    elif event.key == pygame.K_s:
                        machine.goto(buttonPressed, 1)
                    elif event.key == pygame.K_d:
                        machine.goto(buttonPressed, 2)
                    elif event.key == pygame.K_f:
                        machine.goto(buttonPressed, 3)
                    elif event.key == pygame.K_q:
                        machine.move(buttonPressed, 1)
                    elif event.key == pygame.K_w:
                        machine.move(buttonPressed, -1)

            elif event.type == pygame.MOUSEBUTTONDOWN and event.button == 1:
                mouse_pos = pygame.mouse.get_pos()
                for i, button in enumerate(buttons):
                    button.isPressed = False
                    if is_mouse_over(mouse_pos, button.rect):
                        buttonPressed = i
                        button.isPressed = True
                # # Check if the left mouse button is clicked on the buttons
                if is_mouse_over(mouse_pos, pygame.Rect(150, 150, 100, 50)):
                    print("Button 1 clicked!")
                elif is_mouse_over(mouse_pos, pygame.Rect(150, 220, 100, 50)):
                    print("Button 2 clicked!")

        # tick the servos
        machine.tick()
        moves1.tick()
        moves2.tick()
        servoMoves.tick()
        # Clear the screen
        screen.fill(WHITE)

        # Draw the input field
        pygame.draw.rect(screen, GRAY, pygame.Rect(10, 10, 200, 40))
        draw_text(number_input, 20, 20, BLACK)

        # Draw buttons
        for button in buttons:
            button.draw(screen)
        # draw_button(pygame.Rect(150, 150, 100, 50), BLACK, "Button 1")
        # draw_button(pygame.Rect(150, 220, 100, 50), BLACK, "Button 2")

        # Update the display
        pygame.display.flip()

        clock.tick(20)
