/***************************************************************************
**
**  This file is part of QGpCoreMath.
**
**  This library is free software; you can redistribute it and/or
**  modify it under the terms of the GNU Lesser General Public
**  License as published by the Free Software Foundation; either
**  version 2.1 of the License, or (at your option) any later version.
**
**  This file is distributed in the hope that it will be useful, but WITHOUT
**  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
**  FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
**  License for more details.
**
**  You should have received a copy of the GNU Lesser General Public
**  License along with this library; if not, write to the Free Software
**  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**
**  See http://www.geopsy.org for more information.
**
**  Created: 2008-07-29
**  Copyright: 2008-2019
**    Marc Wathelet
**    Marc Wathelet (LGIT, Grenoble, France)
**
***************************************************************************/


#include <math.h>

#include "PointLocate.h"
#include "Angle.h"
#include "Circle.h"
#include "Line2D.h"

namespace QGpCoreMath {

/*!
  \class PointLocate PointLocate.h
  \brief Brief description of class still missing

  Full description of class still missing
*/

/*!
  Description of constructor still missing
*/
PointLocate::PointLocate()
{
  TRACE;
  for(int i=0;i < 3;i++) {
    _isDistance[i]=false;
    _isAzimuth[i]=false;
  }
}

/*!
  Description of destructor still missing
*/
PointLocate::~PointLocate()
{
  TRACE;
}

void PointLocate::setReference(int index, const QString& s)
{
  TRACE;
  ASSERT(index<3);
  _references[index]=s;
}

void PointLocate::setDistance(int index, double d)
{
  TRACE;
  ASSERT(index<3);
  _distances[index]=d;
  _isDistance[index]=true;
}

void PointLocate::setAzimuth(int index, double az)
{
  TRACE;
  ASSERT(index<3);
  _azimuths[index]=az;
  _isAzimuth[index]=true;
}

void PointLocate::setDistance(int index)
{
  TRACE;
  ASSERT(index<3);
  _isDistance[index]=false;
}

void PointLocate::setAzimuth(int index)
{
  TRACE;
  ASSERT(index<3);
  _isAzimuth[index]=false;
}

QString PointLocate::reference(int index) const
{
  TRACE;
  ASSERT(index<3);
  return _references[index];
}

bool PointLocate::isDistance(int index) const
{
  TRACE;
  ASSERT(index<3);
  return _isDistance[index];
}

double PointLocate::distance(int index) const
{
  TRACE;
  ASSERT(index<3);
  return _distances[index];
}

bool PointLocate::isAzimuth(int index) const
{
  TRACE;
  ASSERT(index<3);
  return _isAzimuth[index];
}

double PointLocate::azimuth(int index) const
{
  TRACE;
  ASSERT(index<3);
  return _azimuths[index];
}

void PointLocate::availableInformation(int& distCount, int& azimuthCount, int& id, int& iaz) const
{
  TRACE;
  distCount=0;
  azimuthCount=0;
  for(int i=0;i < 3;i++) {
    if(_isDistance[i]) {
      distCount++;
      id=i;
    }
    if(_isAzimuth[i]) {
      azimuthCount++;
      iaz=i;
    }
  }
}

/*!
  Return this location as a string
*/
QString PointLocate::toString() const
{
  TRACE;
  // Check available information
  int distCount, azimuthCount, id, iaz;
  availableInformation(distCount, azimuthCount, id, iaz);
  if(azimuthCount==0) {
    if(distCount==3) {
      return QString("3d__%1:%2m:%3:%4m:%5:%6m")
            .arg(_references[0])
            .arg(_distances[0])
            .arg(_references[1])
            .arg(_distances[1])
            .arg(_references[2])
            .arg(_distances[2]);
    } else if(distCount==2) {
      return QString("2d__%1:%2m:%3:%4m:%5")
            .arg(_references[0])
            .arg(_distances[0])
            .arg(_references[1])
            .arg(_distances[1])
            .arg(_solutionIndex);
    }
  } else if(azimuthCount==3) {
    return QString("3az_%1:%2:%3:%4:%5:%6")
            .arg(_references[0])
            .arg(Angle::radiansToDegrees(_azimuths[0]))
            .arg(_references[1])
            .arg(Angle::radiansToDegrees(_azimuths[1]))
            .arg(_references[2])
            .arg(Angle::radiansToDegrees(_azimuths[2]));
  } else if(distCount==1 && azimuthCount==1 && id==iaz) {
    return QString("1___%1:%2m:%3")
            .arg(_references[id])
            .arg(_distances[id])
            .arg(Angle::radiansToDegrees(_azimuths[id]));
  }
  return QString();
}

Point PointLocate::position(const QMap<QString, const Point *>& references, Point& uncertainty, int& nSolutions) const
{
  TRACE;
  nSolutions=0;
  Point solution(0, 0, 0);
  uncertainty=Point(0, 0, 0);
  // Check available information
  int distCount, azimuthCount, id, iaz;
  availableInformation(distCount, azimuthCount, id, iaz);
  if(azimuthCount==0) {
    if(distCount==3) {
      const Point * refPoints[3];
      refPoints[0]=references[_references[0]];
      if(!refPoints[0]) return solution;
      refPoints[1]=references[_references[1]];
      if(!refPoints[1]) return solution;
      refPoints[2]=references[_references[2]];
      if(!refPoints[2]) return solution;
      Circle c1(refPoints[0]->x(), refPoints[0]->y(), _distances[0]);
      Circle c2(refPoints[1]->x(), refPoints[1]->y(), _distances[1]);
      Circle c3(refPoints[2]->x(), refPoints[2]->y(), _distances[2]);
      bool ok;
      int n=0;
      Point2D p;
      p=c1.intersect(c2, ok, c3);
      if(ok) {solution += p; uncertainty += p * p; n++;}
      p=c1.intersect(c3, ok, c2);
      if(ok) {solution += p; uncertainty += p * p; n++;}
      p=c2.intersect(c3, ok, c1);
      if(ok) {solution += p; uncertainty += p * p; n++;}
      if(n!=0) {
        solution/=n;
        uncertainty.setX(::sqrt(uncertainty.x()/n-solution.x()*solution.x()));
        uncertainty.setY(::sqrt(uncertainty.y()/n-solution.y()*solution.y()));
        nSolutions=1;
      }
    } else if(distCount==2) {
      const Point * refPoints[2];
      refPoints[0]=references[_references[0]];
      if(!refPoints[0]) return solution;
      refPoints[1]=references[_references[1]];
      if(!refPoints[1]) return solution;
      Circle c1(refPoints[0]->x(), refPoints[0]->y(), _distances[0]);
      Circle c2(refPoints[1]->x(), refPoints[1]->y(), _distances[1]);
      QVector<Point2D> solutions=c1.intersect(c2);
      nSolutions=solutions.count();
      switch(solutions.count()) {
      case 1:
        solution=solutions[0];
        break;
      case 2:
        solution=solutions[_solutionIndex];
        break;
      default:
        break;
      }
    }
  } else if(azimuthCount==3) {
    const Point * refPoints[3];
    refPoints[0]=references[_references[0]];
    if(!refPoints[0]) return solution;
    refPoints[1]=references[_references[1]];
    if(!refPoints[1]) return solution;
    refPoints[2]=references[_references[2]];
    if(!refPoints[2]) return solution;
    Line2D l1(refPoints[0]->x(), refPoints[0]->y(), _azimuths[0]);
    Line2D l2(refPoints[1]->x(), refPoints[1]->y(), _azimuths[1]);
    Line2D l3(refPoints[2]->x(), refPoints[2]->y(), _azimuths[2]);
    bool ok;
    int n=0;
    Point2D p;
    p=l1.intersect(l2, ok);
    if(ok) {solution += p; uncertainty += p * p; n++;}
    p=l1.intersect(l3, ok);
    if(ok) {solution += p; uncertainty += p * p; n++;}
    p=l2.intersect(l3, ok);
    if(ok) {solution += p; uncertainty += p * p; n++;}
    if(n!=0) {
      solution/=n;
      uncertainty.setX(::sqrt(uncertainty.x()/n-solution.x()*solution.x()));
      uncertainty.setY(::sqrt(uncertainty.y()/n-solution.y()*solution.y()));
      nSolutions=1;
    }
  } else if(distCount==1 && azimuthCount==1 && id==iaz) {
    const Point * refPoint;
    refPoint=references[_references[id]];
    if(!refPoint) return solution;
    solution.setX(refPoint->x()+_distances[id]*::cos(0.5 * M_PI - _azimuths[id]));
    solution.setY(refPoint->y()+_distances[id]*::sin(0.5 * M_PI - _azimuths[id]));
    nSolutions=1;
  }
  return solution;
}

/*!
  Initialize from a string (format returned by toString()).
  Return false in case of error.
*/
bool PointLocate::fromString(const QString& s)
{
  TRACE;
  QString head=s.left(4);
  QString info=s.right(s.length() - 4);
  if(head=="3d__") {
    _references[0]=info.section(':', 0, 0);
    head=info.section(':', 1, 1);
    _distances[0]=head.left(head.length() - 1).toDouble();
    _isDistance[0]=true;
    _isAzimuth[0]=false;

    _references[1]=info.section(':', 2, 2);
    head=info.section(':', 3, 3);
    _distances[1]=head.left(head.length() - 1).toDouble();
    _isDistance[1]=true;
    _isAzimuth[1]=false;

    _references[2]=info.section(':', 4, 4);
    head=info.section(':', 5, 5);
    _distances[2]=head.left(head.length() - 1).toDouble();
    _isDistance[2]=true;
    _isAzimuth[2]=false;
  } else if(head=="2d__") {
    _references[0]=info.section(':', 0, 0);
    head=info.section(':', 1, 1);
    _distances[0]=head.left(head.length() - 1).toDouble();
    _isDistance[0]=true;
    _isAzimuth[0]=false;

    _references[1]=info.section(':', 2, 2);
    head=info.section(':', 3, 3);
    _distances[1]=head.left(head.length() - 1).toDouble();
    _isDistance[1]=true;
    _isAzimuth[1]=false;

    _solutionIndex=info.section(':', 4, 4).toInt();
    _isDistance[2]=false;
    _isAzimuth[2]=false;
  } else if(head=="3az_") {
    _references[0]=info.section(':', 0, 0);
    head=info.section(':', 1, 1);
    _azimuths[0]=Angle::degreesToRadians(head.left(head.length()).toDouble());
    _isDistance[0]=false;
    _isAzimuth[0]=true;

    _references[1]=info.section(':', 2, 2);
    head=info.section(':', 3, 3);
    _azimuths[1]=Angle::degreesToRadians(head.left(head.length()).toDouble());
    _isDistance[1]=false;
    _isAzimuth[1]=true;

    _references[2]=info.section(':', 4, 4);
    head=info.section(':', 5, 5);
    _azimuths[2]=Angle::degreesToRadians(head.left(head.length()).toDouble());
    _isDistance[2]=false;
    _isAzimuth[2]=true;
  } else if(head=="1___") {
    _references[0]=info.section(':', 0, 0);
    head=info.section(':', 1, 1);
    _distances[0]=head.left(head.length() - 1).toDouble();
    head=info.section(':', 2, 2);
    _azimuths[0]=Angle::degreesToRadians(head.left(head.length()).toDouble());
    _isDistance[0]=true;
    _isAzimuth[0]=true;
    _isDistance[1]=false;
    _isAzimuth[1]=false;
    _isDistance[2]=false;
    _isAzimuth[2]=false;
  } else {
    return false;
  }
  return true;
}

} // namespace QGpCoreMath
