"""A weighted least-squares pose graph in one dimension; not a complete SLAM system."""
import json
import numpy as np


def main():
    truth = np.array([0., 1., 2., 3., 2., 1., 0.])
    odom = np.diff(truth) + 0.12
    rows, values = [], []
    for i, delta in enumerate(odom):
        row = np.zeros(len(truth))
        row[i], row[i + 1] = -1 / 0.1, 1 / 0.1
        rows.append(row)
        values.append(delta / 0.1)
    unanchored_rank = int(np.linalg.matrix_rank(np.array(rows)))
    anchor = np.zeros(len(truth))
    anchor[0] = 1 / 0.001
    rows.append(anchor)
    values.append(0.0)
    before, *_ = np.linalg.lstsq(np.array(rows), np.array(values), rcond=None)
    loop = np.zeros(len(truth))
    loop[0], loop[-1] = -1 / 0.05, 1 / 0.05
    rows.append(loop)
    values.append(0.0)
    after, *_ = np.linalg.lstsq(np.array(rows), np.array(values), rcond=None)
    report = {"truth_m": truth.tolist(), "before_loop_m": before.tolist(),
              "after_loop_m": after.tolist(), "unanchored_rank": unanchored_rank,
              "variables": len(truth), "before_rmse_m": float(np.sqrt(np.mean((before-truth)**2))),
              "after_rmse_m": float(np.sqrt(np.mean((after-truth)**2)))}
    assert unanchored_rank == len(truth) - 1
    assert report["after_rmse_m"] < report["before_rmse_m"]
    print(json.dumps(report, indent=2))


if __name__ == "__main__":
    main()