# Path: thrifty/pos_est.py
# SPEED_OF_LIGHT = tdoa_est.SPEED_OF_LIGHT
# POSITION_INFO_DTYPE = {
#     'names': ('group_id', 'timestamp', 'tx', 'dop', 'snr', 'x', 'y', 'z'),
#     'formats': ('i4', 'f8', 'i4', 'f8', 'f8', 'f8', 'f8', 'f8')
# }
# MAX_DIST = 10e3
#     G = pos_rx0 / dist0[:, None] - pos_rx1 / dist1[:, None]
#         H = np.linalg.inv(H_inv)
#         H = None
# class EstimationError(Exception):
# def solve_1d(tdoa_array, rx_pos):
# def solve_analytically(tdoa_array, rx_pos):
# def solve_numerically(tdoa_array, rx_pos):
#     def model(pos):
#     def jac(pos):
# def dop_matrix(pos, rx_pos, rx_pairs):
# def dop(pos, rx_pos, rx_pairs):
# def solve(tdoa_groups, rx_pos):
# def save_positions(output, results):
# def load_positions(fname):
# def _main():
# 
# Path: thrifty/tdoa_est.py
# SPEED_OF_LIGHT = 2.997e8
# MAX_TDOA = 30e3 / SPEED_OF_LIGHT
# TDOA_DTYPE = {'names': ('rx0', 'rx1', 'tdoa', 'snr', 'model_quality',
#                         'det0_idx', 'det1_idx'),
#               'formats': ('i4', 'i4', 'f8', 'f8', 'f8', 'i4', 'i4')}
# MATRIX_DTYPE = {'names': ('group_id', 'timestamp', 'tx') + TDOA_DTYPE['names'],
#                 'formats': ('i4', 'f8', 'i4') + TDOA_DTYPE['formats']}
# def make_detection_extractor(detections, matches):
#     def extract(rxid0, rxid1, timestamp_start, timestamp_stop):
# def estimate_model_quality(model, detection_pairs):
# def build_model_poly(detection_pairs, beacon_sdoa, nominal_sample_rate, deg=2):
#     def evaluate(det0, det1):
# def build_model_weighted_poly(detection_pairs, beacon_sdoa,
#                               nominal_sample_rate, deg=2):
#     def evaluate(det0, det1):
# def find_nearest_value(list_, value):
# def test_find_nearest_value():
# def build_model_nearest(detection_pairs, beacon_sdoa, nominal_sample_rate):
#     def evaluate(det0, det1):
# def build_model_linear(detection_pairs, beacon_sdoa, nominal_sample_rate):
#     def evaluate(det0, det1):
# def _dist(vector1, vector2):
# def estimate_tdoas(detections, matches, window_size,
#                    beacon_pos, rx_pos, sample_rate,
#                    model_builder=default_model, model_params=None):
#     def _beacon_tdoa(rxid0, rxid1, beaconid):
# def save_tdoa_groups(output, tdoa_groups):
# def load_tdoa_matrix(fname):
# def groups_to_matrix(groups):
# def load_tdoa_groups(fname):
# def load_pos_config(file_):
# def _main():


import itertools
import numpy as np
from thrifty import pos_est
from thrifty import tdoa_est
    1: [100, 0],
    2: [0, 100],
    3: [110, 90],
}

TX_POS = [30, 40]


def gen_tdoa_data(rx_pos, tx_pos):
    tdoas = []
    for rx0, rx1 in itertools.combinations(rx_pos.keys(), 2):
        rx0_pos = np.array(rx_pos[rx0])
        rx1_pos = np.array(rx_pos[rx1])
        t0 = np.linalg.norm(rx0_pos - tx_pos)
        t1 = np.linalg.norm(rx1_pos - tx_pos)
        tdoa = (t0 - t1) / SPEED_OF_LIGHT
        tdoas.append((rx0, rx1, tdoa, 0, 0, 0, 0))
    return np.array(tdoas, dtype=tdoa_est.TDOA_DTYPE)

tdoa_array = gen_tdoa_data(RX_POS, TX_POS)
print(pos_est.solve_numerically(tdoa_array, RX_POS))  # TODO: assert all close

rx_pairs = zip(tdoa_array['rx0'], tdoa_array['rx1'])
print(pos_est.dop(TX_POS, RX_POS, rx_pairs))


def test_1d_dop():
    tx_pos = [5]
    rx_pos = {0: [0], 1: [10]}
    rx_pairs = [(0, 1)]

# Path: thrifty/pos_est.py
# SPEED_OF_LIGHT = tdoa_est.SPEED_OF_LIGHT
# POSITION_INFO_DTYPE = {
#     'names': ('group_id', 'timestamp', 'tx', 'dop', 'snr', 'x', 'y', 'z'),
#     'formats': ('i4', 'f8', 'i4', 'f8', 'f8', 'f8', 'f8', 'f8')
# }
# MAX_DIST = 10e3
#     G = pos_rx0 / dist0[:, None] - pos_rx1 / dist1[:, None]
#         H = np.linalg.inv(H_inv)
#         H = None
# class EstimationError(Exception):
# def solve_1d(tdoa_array, rx_pos):
# def solve_analytically(tdoa_array, rx_pos):
# def solve_numerically(tdoa_array, rx_pos):
#     def model(pos):
#     def jac(pos):
# def dop_matrix(pos, rx_pos, rx_pairs):
# def dop(pos, rx_pos, rx_pairs):
# def solve(tdoa_groups, rx_pos):
# def save_positions(output, results):
# def load_positions(fname):
# def _main():
# 
# Path: thrifty/tdoa_est.py
# SPEED_OF_LIGHT = 2.997e8
# MAX_TDOA = 30e3 / SPEED_OF_LIGHT
# TDOA_DTYPE = {'names': ('rx0', 'rx1', 'tdoa', 'snr', 'model_quality',
#                         'det0_idx', 'det1_idx'),
#               'formats': ('i4', 'i4', 'f8', 'f8', 'f8', 'i4', 'i4')}
# MATRIX_DTYPE = {'names': ('group_id', 'timestamp', 'tx') + TDOA_DTYPE['names'],
#                 'formats': ('i4', 'f8', 'i4') + TDOA_DTYPE['formats']}
# def make_detection_extractor(detections, matches):
#     def extract(rxid0, rxid1, timestamp_start, timestamp_stop):
# def estimate_model_quality(model, detection_pairs):
# def build_model_poly(detection_pairs, beacon_sdoa, nominal_sample_rate, deg=2):
#     def evaluate(det0, det1):
# def build_model_weighted_poly(detection_pairs, beacon_sdoa,
#                               nominal_sample_rate, deg=2):
#     def evaluate(det0, det1):
# def find_nearest_value(list_, value):
# def test_find_nearest_value():
# def build_model_nearest(detection_pairs, beacon_sdoa, nominal_sample_rate):
#     def evaluate(det0, det1):
# def build_model_linear(detection_pairs, beacon_sdoa, nominal_sample_rate):
#     def evaluate(det0, det1):
# def _dist(vector1, vector2):
# def estimate_tdoas(detections, matches, window_size,
#                    beacon_pos, rx_pos, sample_rate,
#                    model_builder=default_model, model_params=None):
#     def _beacon_tdoa(rxid0, rxid1, beaconid):
# def save_tdoa_groups(output, tdoa_groups):
# def load_tdoa_matrix(fname):
# def groups_to_matrix(groups):
# def load_tdoa_groups(fname):
# def load_pos_config(file_):
# def _main():


# Path: tests/test_pos_est.py
import itertools
import numpy as np
from thrifty import pos_est
from thrifty import tdoa_est

    1: [100, 0],
    2: [0, 100],
    3: [110, 90],
}

TX_POS = [30, 40]


def gen_tdoa_data(rx_pos, tx_pos):
    tdoas = []
    for rx0, rx1 in itertools.combinations(rx_pos.keys(), 2):
        rx0_pos = np.array(rx_pos[rx0])
        rx1_pos = np.array(rx_pos[rx1])
        t0 = np.linalg.norm(rx0_pos - tx_pos)
        t1 = np.linalg.norm(rx1_pos - tx_pos)
        tdoa = (t0 - t1) / SPEED_OF_LIGHT
        tdoas.append((rx0, rx1, tdoa, 0, 0, 0, 0))
    return np.array(tdoas, dtype=tdoa_est.TDOA_DTYPE)

tdoa_array = gen_tdoa_data(RX_POS, TX_POS)
print(pos_est.solve_numerically(tdoa_array, RX_POS))  # TODO: assert all close

rx_pairs = zip(tdoa_array['rx0'], tdoa_array['rx1'])
print(pos_est.dop(TX_POS, RX_POS, rx_pairs))


def test_1d_dop():
    tx_pos = [5]
    rx_pos = {0: [0], 1: [10]}
    rx_pairs = [(0, 1)]

    assert pos_est.dop(tx_pos, rx_pos, rx_pairs) == 0.5