# -*- coding: utf-8 -*-
"""
@file   star_tracker.py
@brief  Star tracker commanding
@author J. Meier
@date   14.08.2021
"""
import struct

from tmtccmd.config.definitions import QueueCommands

from tmtccmd.tc.packer import TcQueueT
from spacepackets.ecss.tc import PusTelecommand
from pus_tc.service_200_mode import pack_mode_data


class StarTrackerActionIds:
    PING = 0
    BOOT = 1
    REQ_VERSION = 2
    REQ_INTERFACE = 3
    REQ_TIME = 4
    REQ_POWER = 11
    SWITCH_TO_BOOTLOADER_PROGRAM = 7
    DOWNLOAD_IMAGE = 9
    UPLOAD_IMAGE = 10
    SUBSCRIBE_TO_TM = 18
    REQ_SOLUTION = 24
    REQ_TEMPERATURE = 25
    REQ_HISTOGRAM = 28
    REQ_CONTRAST = 29
    LIMITS = 40
    MOUNTING = 41
    CAMERA = 42
    BLOB = 43
    CENTROIDING = 44
    LISA = 45
    MATCHING = 46
    TRACKING = 47
    VALIDATION = 48
    ALGO = 49
    CHECKSUM = 50
    READ = 51
    WRITE = 52
    TAKE_IMAGE = 15
    STOP_IMAGE_LOADER = 55
    RESET_ERROR = 56
    CHANGE_DOWNLOAD_IMAGE = 57
    SET_JSON_FILE_NAME = 58
    SET_READ_FILENAME = 59


class ImagePathDefs:
    uploadFile = "/mnt/sd0/startracker/gemma.bin"
    downloadFile = "test_image.bin"
    downloadPath = "/mnt/sd0/startracker"
    jsonFile = "/mnt/sd0/startracker/test.json"
    flashFile = "/mnt/sd0/startracker/flash.bin"
    flashReadPath = "/mnt/sd0/startracker"


class Region:
    # Definition according to datasheet (which turned out to be partially faulty)
    BOOTLOADER = 0
    STAR_TRACKER_FIRMWARE = 1
    FREE_1 = 2
    FREE_2 = 3


class PartitionSize:
    BOOTLOADER = 128000
    STAR_TRACKER_FIRMWARE = 896000
    FREE_1 = 128000
    FREE_2 = 896000


def pack_star_tracker_commands(object_id: bytearray, tc_queue: TcQueueT, op_code: str) -> TcQueueT:
    tc_queue.appendleft(
        (QueueCommands.PRINT,
         "Generate command for star tracker with object id: 0x" + object_id.hex())
    )

    if op_code == "0":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Mode On"))
        command = pack_mode_data(object_id, 1, 0)
        command = PusTelecommand(service=200, subservice=1, ssc=10, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "1":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Mode Normal"))
        command = pack_mode_data(object_id, 2, 0)
        command = PusTelecommand(service=200, subservice=1, ssc=11, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "2":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Mode Off"))
        command = pack_mode_data(object_id, 0, 0)
        command = PusTelecommand(service=200, subservice=1, ssc=12, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "3":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Mode Raw"))
        command = pack_mode_data(object_id, 3, 0)
        command = PusTelecommand(service=200, subservice=1, ssc=13, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "4":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Ping"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.PING)
        command = PusTelecommand(service=8, subservice=128, ssc=30, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "5":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker:  Switch to bootloader program"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.SWITCH_TO_BOOTLOADER_PROGRAM)
        command = PusTelecommand(service=8, subservice=128, ssc=31, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "6":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Temperature request"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.REQ_TEMPERATURE)
        command = PusTelecommand(service=8, subservice=128, ssc=32, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "7":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Request version"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.REQ_VERSION)
        command = PusTelecommand(service=8, subservice=128, ssc=33, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "8":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Request interface"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.REQ_INTERFACE)
        command = PusTelecommand(service=8, subservice=128, ssc=34, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "9":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Request power"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.REQ_POWER)
        command = PusTelecommand(service=8, subservice=128, ssc=35, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "10":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Subscribe to telemetry"))
        tm_id = int(input("Specify Id of tm: "))
        command = object_id + struct.pack('!I', StarTrackerActionIds.SUBSCRIBE_TO_TM) + struct.pack('B', tm_id)
        command = PusTelecommand(service=8, subservice=128, ssc=36, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "11":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Boot"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.BOOT)
        command = PusTelecommand(service=8, subservice=128, ssc=37, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "12":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Request time"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.REQ_TIME)
        command = PusTelecommand(service=8, subservice=128, ssc=38, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "13":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Request solution"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.REQ_SOLUTION)
        command = PusTelecommand(service=8, subservice=128, ssc=39, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "14":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Upload image"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.UPLOAD_IMAGE) + \
                  bytearray(ImagePathDefs.uploadFile, 'utf-8')
        command = PusTelecommand(service=8, subservice=128, ssc=40, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "15":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Download image"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.DOWNLOAD_IMAGE) + \
                  bytearray(ImagePathDefs.downloadPath, 'utf-8')
        command = PusTelecommand(service=8, subservice=128, ssc=53, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "16":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Set limits"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.LIMITS) + \
                  bytearray(ImagePathDefs.jsonFile, 'utf-8')
        command = PusTelecommand(service=8, subservice=128, ssc=42, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "17":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Set tracking parameters"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.TRACKING) + \
                  bytearray(ImagePathDefs.jsonFile, 'utf-8')
        command = PusTelecommand(service=8, subservice=128, ssc=43, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "18":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Mounting"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.MOUNTING) + \
                  bytearray(ImagePathDefs.jsonFile, 'utf-8')
        command = PusTelecommand(service=8, subservice=128, ssc=44, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "19":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Camera"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.CAMERA) + \
                  bytearray(ImagePathDefs.jsonFile, 'utf-8')
        command = PusTelecommand(service=8, subservice=128, ssc=45, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "20":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Blob"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.BLOB) + \
                  bytearray(ImagePathDefs.jsonFile, 'utf-8')
        command = PusTelecommand(service=8, subservice=128, ssc=46, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "21":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Centroiding"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.CENTROIDING) + \
                  bytearray(ImagePathDefs.jsonFile, 'utf-8')
        command = PusTelecommand(service=8, subservice=128, ssc=47, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "22":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: LISA"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.LISA) + \
                  bytearray(ImagePathDefs.jsonFile, 'utf-8')
        command = PusTelecommand(service=8, subservice=128, ssc=48, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "23":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Matching"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.MATCHING) + \
                  bytearray(ImagePathDefs.jsonFile, 'utf-8')
        command = PusTelecommand(service=8, subservice=128, ssc=49, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "24":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Validation"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.VALIDATION) + \
                  bytearray(ImagePathDefs.jsonFile, 'utf-8')
        command = PusTelecommand(service=8, subservice=128, ssc=50, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "25":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Algo"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.ALGO) + \
                  bytearray(ImagePathDefs.jsonFile, 'utf-8')
        command = PusTelecommand(service=8, subservice=128, ssc=51, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "26":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Take image"))
        actionid = int(input("Specify parameter ID (nominal - 4): "))
        command = object_id + struct.pack('!I', StarTrackerActionIds.TAKE_IMAGE) + struct.pack('!B', actionid)
        command = PusTelecommand(service=8, subservice=128, ssc=52, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "27":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Stop image loader"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.STOP_IMAGE_LOADER)
        command = PusTelecommand(service=8, subservice=128, ssc=54, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "28":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Reset error signal"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.RESET_ERROR)
        command = PusTelecommand(service=8, subservice=128, ssc=54, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "29":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Set name of download image"))
        filename = input("Specify download image name: ")
        command = object_id + struct.pack('!I', StarTrackerActionIds.CHANGE_DOWNLOAD_IMAGE) + \
                  bytearray(filename, 'utf-8')
        command = PusTelecommand(service=8, subservice=128, ssc=54, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "30":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Request histogram"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.REQ_HISTOGRAM)
        command = PusTelecommand(service=8, subservice=128, ssc=55, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "31":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Request contrast"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.REQ_CONTRAST)
        command = PusTelecommand(service=8, subservice=128, ssc=56, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "32":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Set json filename"))
        command = object_id + struct.pack('!I', StarTrackerActionIds.SET_JSON_FILE_NAME) + \
                  bytearray(ImagePathDefs.jsonFile, 'utf-8')
        command = PusTelecommand(service=8, subservice=128, ssc=57, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "33":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Write"))
        command = pack_write_command(object_id)
        command = PusTelecommand(service=8, subservice=128, ssc=58, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "34":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Read"))
        command = pack_read_command(object_id)
        command = PusTelecommand(service=8, subservice=128, ssc=59, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "35":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Set read filename"))
        filename = input("Specify filename: ")
        command = object_id + StarTrackerActionIds.SET_READ_FILENAME + bytearray(filename, 'utf-8')
        command = PusTelecommand(service=8, subservice=128, ssc=60, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())
    if op_code == "36":
        tc_queue.appendleft((QueueCommands.PRINT, "Star tracker: Get checksum"))
        command = pack_checksum_command(object_id)
        command = PusTelecommand(service=8, subservice=128, ssc=61, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())


def pack_write_command(object_id: bytearray) -> bytearray:
    region = Region.FREE_1
    address = 0
    command = object_id + struct.pack('!I', StarTrackerActionIds.WRITE) + struct.pack('!B', region) + \
              struct.pack('!I', address) + bytearray(ImagePathDefs.flashFile, 'utf-8')
    return command


def pack_read_command(object_id: bytearray) -> bytearray:
    region = Region.STAR_TRACKER_FIRMWARE
    address = 0
    size = PartitionSize.STAR_TRACKER_FIRMWARE
    command = object_id + struct.pack('!I', StarTrackerActionIds.READ) + struct.pack('!B', region) + \
              struct.pack('!I', address) + struct.pack('!I', size) + bytearray(ImagePathDefs.flashReadPath, 'utf-8')
    return command


def pack_checksum_command(object_id: bytearray) -> bytearray:
    region = Region.STAR_TRACKER_FIRMWARE
    address = 0
    size = PartitionSize.STAR_TRACKER_FIRMWARE
    command = object_id + struct.pack('!I', StarTrackerActionIds.CHECKSUM) + struct.pack('!B', region) + \
              struct.pack('!I', address) + struct.pack('!I', size)
    return command