// $Id: Distances.cpp,v 1.2 2006/08/02 03:39:28 pkaz Exp $
//
// This program computes the distances between all pairs of points in the specified
// input file and compares them to the corresponding distances computed from the CNC
// coordinates (ground truth).

// system includes
#include <iostream>
#include <stdio.h>

// cisst includes
#include <cisstNumerical/nmrSVD.h>

#include "DataSet.h"
#include "Phantom.h"

using namespace std;

// DistanceDataSet is derived from DataSet.  It adds the method for
// computing the distances.
class DistanceDataSet: public DataSet {
protected:
public:
    DistanceDataSet(int maxpts) : DataSet(maxpts) {}
    ~DistanceDataSet() {}
    void ComputeDistances(bool do_table);
};

// ComputeDistances: compute the distances between each pair of points.
void DistanceDataSet::ComputeDistances(bool do_table)
{
    int i,j;
    double max_err = 0;
    double avg_err = 0;
    double avg_err_mag = 0;
    int num_dists = 0;
    printf("Distance errors (robot - cnc):\n");

    if (do_table) {
        printf("\n     ");
        for (i=1; i < nrpoints; i++) {
            printf("   %2s   ", Phantom::GetName(nameIndex[i]));
        }
        printf("\n");
    }
    for (i=0; i < nrpoints-1; i++) {
        if (do_table) {
            printf(" %2s  ", Phantom::GetName(nameIndex[i]));
            for (j=1; j < i+1; j++)
                printf("        ");
        }
        for (j=i+1; j < nrpoints; j++) {
            num_dists++;
            double rob_dist = (data[0][i]-data[0][j]).Norm();
            double cnc_dist = (data[1][i]-data[1][j]).Norm();
            double dist_err = rob_dist - cnc_dist;
            avg_err += dist_err;
            avg_err_mag += fabs(dist_err);
            if (fabs(dist_err) > fabs(max_err))
                max_err = dist_err;
            if (do_table)
                printf(" %6.3lf ", dist_err);
            else {
                printf("%2s - %2s:  %8.3lf (cnc_dist = %8.3lf, rob_dist = %8.3lf)\n", 
                   Phantom::GetName(i), Phantom::GetName(j), dist_err, cnc_dist, rob_dist);
            }
        }
        if (do_table)
            printf("\n");
    }
    printf("\n");
    if (num_dists > 0) {
        avg_err /= num_dists;
        avg_err_mag /= num_dists;
        printf("Number of distances = %d\n", num_dists);
        printf("Maximum error   = %8.3lf\n", max_err);
        printf("Average error   = %8.3lf\n", avg_err);
        printf("Average |error| = %8.3lf\n", avg_err_mag);
        double std_dev = 0;
        for (i=0; i < nrpoints; i++) {
            for (j=i+1; j < nrpoints; j++) {
                double rob_dist = (data[0][i]-data[0][j]).Norm();
                double cnc_dist = (data[1][i]-data[1][j]).Norm();
                double dist_err_mag = fabs(rob_dist - cnc_dist);
                std_dev += (dist_err_mag - avg_err_mag)*(dist_err_mag - avg_err_mag);
            }
        }
        std_dev = sqrt(std_dev/(num_dists-1));
        printf("Standard deviation of |error| = %8.3lf\n", std_dev);
    }
}

int main(int argc, char* argv[])
{
    if (argc < 2) {
        cout << endl << "Syntax:  " << argv[0] << " input_file [list]" << endl << endl;
        cout << "Displays distance errors in tabular format (default) or in" << endl;
        cout << "single column list if \"list\" option specified" << endl;
        return 0;
    }

    bool do_table = true;;
    if ((argc == 3) && (strcmp(argv[2], "list") == 0))
        do_table = false;

    DistanceDataSet DS(Phantom::GetNumPts());
    DS.ReadFromFile(0, argv[1]);
    DS.ReadFromArray(1, Phantom::GetCNC());
    DS.FinalizeInput();
    DS.ComputeDistances(do_table);
}