/**********************************************************************
 * File:        linlsq.cpp  (Formerly llsq.c)
 * Description: Linear Least squares fitting code.
 * Author:		Ray Smith
 * Created:		Thu Sep 12 08:44:51 BST 1991
 *
 * (C) Copyright 1991, Hewlett-Packard Ltd.
 ** Licensed under the Apache License, Version 2.0 (the "License");
 ** you may not use this file except in compliance with the License.
 ** You may obtain a copy of the License at
 ** http://www.apache.org/licenses/LICENSE-2.0
 ** Unless required by applicable law or agreed to in writing, software
 ** distributed under the License is distributed on an "AS IS" BASIS,
 ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 ** See the License for the specific language governing permissions and
 ** limitations under the License.
 *
 **********************************************************************/

#include "mfcpch.h"
#include          <stdio.h>
#include          <math.h>
#include          "errcode.h"
#include          "linlsq.h"

#ifndef __UNIX__
#define M_PI        3.14159265359
#endif

const ERRCODE EMPTY_LLSQ = "Can't delete from an empty LLSQ";

#define EXTERN

EXTERN double_VAR (pdlsq_posdir_ratio, 4e-6, "Mult of dir to cf pos");
EXTERN double_VAR (pdlsq_threshold_angleavg, 0.1666666,
"Frac of pi for simple fit");

/**********************************************************************
 * LLSQ::clear
 *
 * Function to initialize a LLSQ.
 **********************************************************************/

void LLSQ::clear() {  //initialize
  n = 0;                         //no elements
  sigx = 0;                      //update accumulators
  sigy = 0;
  sigxx = 0;
  sigxy = 0;
  sigyy = 0;
}


/**********************************************************************
 * LLSQ::add
 *
 * Add an element to the accumulator.
 **********************************************************************/

void LLSQ::add(           //add an element
               double x,  //xcoord
               double y   //ycoord
              ) {
  n++;                           //count elements
  sigx += x;                     //update accumulators
  sigy += y;
  sigxx += x * x;
  sigxy += x * y;
  sigyy += y * y;
}


/**********************************************************************
 * LLSQ::remove
 *
 * Delete an element from the acculuator.
 **********************************************************************/

void LLSQ::remove(           //delete an element
                  double x,  //xcoord
                  double y   //ycoord
                 ) {
  if (n <= 0)
                                 //illegal
    EMPTY_LLSQ.error ("LLSQ::remove", ABORT, NULL);
  n--;                           //count elements
  sigx -= x;                     //update accumulators
  sigy -= y;
  sigxx -= x * x;
  sigxy -= x * y;
  sigyy -= y * y;
}


/**********************************************************************
 * LLSQ::m
 *
 * Return the gradient of the line fit.
 **********************************************************************/

double LLSQ::m() {  //get gradient
  if (n > 1)
    return (sigxy - sigx * sigy / n) / (sigxx - sigx * sigx / n);
  else
    return 0;                    //too little
}


/**********************************************************************
 * LLSQ::c
 *
 * Return the constant of the line fit.
 **********************************************************************/

double LLSQ::c(          //get constant
               double m  //gradient to fit with
              ) {
  if (n > 0)
    return (sigy - m * sigx) / n;
  else
    return 0;                    //too little
}


/**********************************************************************
 * LLSQ::rms
 *
 * Return the rms error of the fit.
 **********************************************************************/

double LLSQ::rms(           //get error
                 double m,  //gradient to fit with
                 double c   //constant to fit with
                ) {
  double error;                  //total error

  if (n > 0) {
    error =
      sigyy + m * (m * sigxx + 2 * (c * sigx - sigxy)) + c * (n * c -
      2 * sigy);
    if (error >= 0)
      error = sqrt (error / n);  //sqrt of mean
    else
      error = 0;
  }
  else
    error = 0;                   //too little
  return error;
}


/**********************************************************************
 * LLSQ::spearman
 *
 * Return the spearman correlation coefficient.
 **********************************************************************/

double LLSQ::spearman() {  //get error
  double error;                  //total error

  if (n > 1) {
    error = (sigxx - sigx * sigx / n) * (sigyy - sigy * sigy / n);
    if (error > 0) {
      error = (sigxy - sigx * sigy / n) / sqrt (error);
    }
    else
      error = 1;
  }
  else
    error = 1;                   //too little
  return error;
}


/**********************************************************************
 * PDLSQ::fit
 *
 * Return all the parameters of the fit to pos/dir.
 * The return value is the rms error.
 **********************************************************************/

float PDLSQ::fit(                 //get fit
                 DIR128 &ang,     //output angle
                 float &sin_ang,  //r,theta parameterisation
                 float &cos_ang,
                 float &r) {
  double a, b;                   //itermediates
  double angle;                  //resulting angle
  double avg_angle;              //simple average
  double error;                  //total error
  double sinx, cosx;             //return values

  if (pos.n > 0) {
    a = pos.sigxy - pos.sigx * pos.sigy / pos.n
      + pdlsq_posdir_ratio * dir.sigxy;
    b =
      pos.sigxx - pos.sigyy + (pos.sigy * pos.sigy -
      pos.sigx * pos.sigx) / pos.n +
      pdlsq_posdir_ratio * (dir.sigxx - dir.sigyy);
    if (dir.sigy != 0 || dir.sigx != 0)
      avg_angle = atan2 (dir.sigy, dir.sigx);
    else
      avg_angle = 0;
    if ((a != 0 || b != 0) && pos.n > 1)
      angle = atan2 (2 * a, b) / 2;
    else
      angle = avg_angle;
    error = avg_angle - angle;
    if (error > M_PI / 2) {
      error -= M_PI;
      angle += M_PI;
    }
    if (error < -M_PI / 2) {
      error += M_PI;
      angle -= M_PI;
    }
    if (error > M_PI * pdlsq_threshold_angleavg
      || error < -M_PI * pdlsq_threshold_angleavg)
      angle = avg_angle;         //go simple
                                 //convert direction
    ang = (inT16) (angle * MODULUS / (2 * M_PI));
    sinx = sin (angle);
    cosx = cos (angle);
    r = (sinx * pos.sigx - cosx * pos.sigy) / pos.n;
    //              tprintf("x=%g, y=%g, xx=%g, xy=%g, yy=%g, a=%g, b=%g, ang=%g, r=%g\n",
    //                      pos.sigx,pos.sigy,pos.sigxx,pos.sigxy,pos.sigyy,
    //                      a,b,angle,r);
    error = dir.sigxx * sinx * sinx + dir.sigyy * cosx * cosx
      - 2 * dir.sigxy * sinx * cosx;
    error *= pdlsq_posdir_ratio;
    error += sinx * sinx * pos.sigxx + cosx * cosx * pos.sigyy
      - 2 * sinx * cosx * pos.sigxy
      - 2 * r * (sinx * pos.sigx - cosx * pos.sigy) + r * r * pos.n;
    if (error >= 0)
                                 //rms value
        error = sqrt (error / pos.n);
    else
      error = 0;                 //-0
    sin_ang = sinx;
    cos_ang = cosx;
  }
  else {
    sin_ang = 0.0f;
    cos_ang = 0.0f;
    ang = 0;
    error = 0;                   //too little
  }
  return error;
}