from pathlib import Path, PurePath

import click
import odrive
import yaml

# CONSTANTS
AXIS_STATE_UNDEFINED = 0
AXIS_STATE_IDLE = 1
AXIS_STATE_STARTUP_SEQUENCE = 2
AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3
AXIS_STATE_MOTOR_CALIBRATION = 4
AXIS_STATE_SENSORLESS_CONTROL = 5
AXIS_STATE_ENCODER_INDEX_SEARCH = 6
AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7
AXIS_STATE_CLOSED_LOOP_CONTROL = 8

INPUT_MODE_VEL_RAMP = 2

# OPTIONS

config_option = click.option(
    '-c',
    '--config',
    default=PurePath(Path(__file__).parent, Path('odrive.yaml')),
    type=Path,
    help='A config file for the odrive. Defaults to odrive.yaml.'
)


@click.group()
def cli():
    pass


@cli.command('config-encoder')
@config_option
def config_encoder(**options):
    """Configures an Odrive with a provided .yaml file of config values."""
    odrv0 = odrive.find_any()
    odrv0.axis0.encoder.config.calib_range = 0.05
    odrv0.axis0.requested_state = AXIS_STATE_ENCODER_OFFSET_CALIBRATION
    # Set negative to reverse index search direction
    odrv0.axis1.encoder.config.calib_range = 0.05
    odrv0.axis1.requested_state = AXIS_STATE_ENCODER_OFFSET_CALIBRATION
    print("You're motor encoder should be configured.")
    return 0


class RoboWheelConfig:
    """
    Class for configuring an Odrive axis.
    """

    # Motor KV
    KV_RATING = 10.0

    # min/max phase inductance of motor
    MIN_PHASE_INDUCTANCE = 0.0  # ?
    MAX_PHASE_INDUCTANCE = 0.001  # ?

    # Min/Max phase resistance of motor
    MIN_PHASE_RESISTANCE = 0.0  # ?
    MAX_PHASE_RESISTANCE = 2.5  # ? I think my motor is 1.7 ohm...

    ENCODER_COUNT_PER_REV = 3200.0

    # Number of magnets in the motor divided by 2
    POLE_PAIR_COUNT = 15.0

    # See https://docs.odriverobotics.com/hoverboard
    RESISTANCE_CALIB_MAX_VOLTAGE = 6.0  # HB motors are larger with more resistance so he doubled the 2.0 default
    REQUESTED_CURRENT_RANGE = 35.0  # lower values are more sensitive default was 60
    CURRENT_CONTROL_BANDWIDTH = 1000.0  # Motors are also high inductance so the bandwidth to 100 from the default 1000
    TORQUE_CONSTANT = 8.27 / KV_RATING
    CALIBRATION_CURRENT = 2
    ENABLE_BRAKE_RESISTOR = True


# Encoder steps
"""
odrv0.config.enable_brake_resistor = True  # Using included 2ohm resistor
odrv0.axis0.motor.config.calibration_current = 2 # highest integer value where I heard the square wave was 4
odrv0.axis0.motor.config.pole_pairs = 15
odrv0.axis0.motor.config.resistance_calib_max_voltage = 6  # motor config still worked with this set to 6
odrv0.axis0.motor.config.requested_current_range = 35   # 35 was the lowest value that produced and audible square wave.
odrv0.axis0.motor.config.current_control_bandwidth = 100.0
odrv0.axis0.motor.config.torque_constant = 8.27 / 10  # KV 10
odrv0.axis1.motor.config.calibration_current = 2 # highest integer value where I heard the square wave was 4
odrv0.axis1.motor.config.pole_pairs = 15
odrv0.axis1.motor.config.resistance_calib_max_voltage = 6  # motor config still worked with this set to 6
odrv0.axis1.motor.config.requested_current_range = 35   # 35 was the lowest value that produced and audible square wave.
odrv0.axis1.motor.config.current_control_bandwidth = 100.0
odrv0.axis1.motor.config.torque_constant = 8.27 / 10  # KV 10

# encoder
odrv0.axis0.encoder.config.mode = ENCODER_MODE_INCREMENTAL
odrv0.axis0.encoder.config.cpr = 3200
odrv0.axis0.encoder.config.bandwidth = 1000
odrv0.axis1.encoder.config.mode = ENCODER_MODE_INCREMENTAL
odrv0.axis1.encoder.config.cpr = 3200
odrv0.axis1.encoder.config.bandwidth = 1000

odrv0.axis0.controller.config.input_mode = INPUT_MODE_VEL_RAMP
odrv0.axis0.controller.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL
odrv0.axis0.controller.config.pos_gain = 1
odrv0.axis0.controller.config.vel_gain = 0.002 * odrv0.axis0.motor.config.torque_constant * odrv0.axis0.encoder.config.cpr
odrv0.axis0.controller.config.vel_integrator_gain = 0.01 * odrv0.axis0.motor.config.torque_constant * odrv0.axis0.encoder.config.cpr
odrv0.axis0.controller.config.vel_limit = 5
odrv0.axis1.controller.config.input_mode = INPUT_MODE_VEL_RAMP
odrv0.axis1.controller.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL
odrv0.axis1.controller.config.pos_gain = 1
odrv0.axis1.controller.config.vel_gain = 0.002 * odrv0.axis1.motor.config.torque_constant * odrv0.axis1.encoder.config.cpr
odrv0.axis1.controller.config.vel_integrator_gain = 0.01 * odrv0.axis1.motor.config.torque_constant * odrv0.axis1.encoder.config.cpr
odrv0.axis1.controller.config.vel_limit = 5

# BREAK HERE
odrv0.save_configuration()
odrv0.reboot()

odrv0.axis0.requested_state = AXIS_STATE_MOTOR_CALIBRATION

dump_errors(odrv0) 

odrv0.axis1.requested_state = AXIS_STATE_MOTOR_CALIBRATION

odrv0.axis0.motor.config.pre_calibrated = True
odrv0.axis1.motor.config.pre_calibrated = True

# BREAK HERE
odrv0.save_configuration()
odrv0.reboot()


# Encoder 
odrv0.axis0.encoder.config.calib_range = 0.05
odrv0.axis0.requested_state = AXIS_STATE_ENCODER_OFFSET_CALIBRATION
odrv0.axis1.encoder.config.calib_range = 0.05
odrv0.axis1.requested_state = AXIS_STATE_ENCODER_OFFSET_CALIBRATION

# Movement
odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
odrv0.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
odrv0.axis0.controller.input_vel = 0.5
odrv0.axis1.controller.input_vel = 0.5
# Your motor should spin here
odrv0.axis0.controller.input_vel = 0
odrv0.axis1.controller.input_vel = 0
odrv0.axis0.requested_state = AXIS_STATE_IDLE
odrv0.axis1.requested_state = AXIS_STATE_IDLE
"""

if __name__ == "__main__":
    cli()