/**
  * @RDE_HEADER@
  * Deposition calculation
  * $Id$
  * $HeadURL$
  * $Author$
  * Created by Gerlinde Rudolphus, SLSA, 2013-03-05
  */


#include "vwm_deposition.h"
#include "vwm_math.h"
#include <cmath>

namespace BuildingControlUnit
{
namespace VWM
{

static double getDistPositionCode(char chDeposition)
{
  double dist = 0.0;
  unsigned char absDeposition = static_cast<unsigned char>(chDeposition);

  if (absDeposition > 127)
    absDeposition = 256 - absDeposition;

  if (absDeposition < 28)
    dist = 0.1 * absDeposition;
  else if (absDeposition < 96)
    dist = exp(absDeposition / 27.1828);
  else
  {
    dist = 0.0;
    // ERROR_LOG(m_logger) << "ERROR: Invalid BSM-deposition";
  }

  if (static_cast<unsigned char>(chDeposition) > 127)
    dist = -dist;

  return dist;
}

static double getAngle(double x, double y)
{

  if (qFuzzyIsNull(x) && qFuzzyIsNull(y))
    return 0.0;

  double phi;
  if (!qFuzzyIsNull(x))
  {
    phi = atan(y / x);
    if (x < 0)
    {
      if (y > 0)
        phi += M_PI;
      else
        phi -= M_PI;
    }
  }
  else
  {
    phi = M_PI / 2.0;
    if (y < 0.0)
      phi = -phi;
  }

  return phi;
}


LtsUtils::QPoint3D getDeposition(int deposition1, int deposition2, const LtsUtils::QPoint3D &bsmDirection)
{
  LtsUtils::QPoint3D ret;
  // keine Ablage -> return
  if (deposition1 == 0 && deposition2 == 0)
    return ret;

  const double alpha = 45.0;
  const double dyAlpha = cos(alpha * M_PI / 180.0);
  const double dzAlpha = sin(alpha * M_PI / 180.0);

  const double dy1 =  dyAlpha * getDistPositionCode(deposition1);
  const double dz1 = -dzAlpha * getDistPositionCode(deposition1);
  const double dy2 = -dyAlpha * getDistPositionCode(deposition2);
  const double dz2 = -dzAlpha * getDistPositionCode(deposition2);

  ret.setY(dy1 + dy2);
  ret.setZ(dz1 + dz2);

  const double x = bsmDirection.x();
  const double y = bsmDirection.y();
  const double z = bsmDirection.z();

  const double phi = getAngle(x, y);

  if (qFuzzyIsNull(z))
  {
    //    INFO_LOG(m_logger) << "phi_1=" << 180.0 * phi / PI;
    ret = Math::rotateAroundZAxis(ret, -phi);
  }
  else
  {
    const double theta = getAngle(sqrt(x * x + y * y), z);
    //    INFO_LOG(m_logger) << "phi_2=" << 180.0 * phi / PI << ", theta=" << 180.0 * theta / PI;
    //    INFO_LOG(m_logger) << "pos0 = (" << posBsmDepositon.getPosition3DX()
    //                       << "," << posBsmDepositon.getPosition3DY()
    //                       << "," << posBsmDepositon.getPosition3DZ() << ")";
    ret = Math::rotateAroundYAxis(ret, theta);
    //     INFO_LOG(m_logger) << "pos1 = (" << pos1.getPosition3DX()
    //                        << "," << pos1.getPosition3DY()
    //                        << "," << pos1.getPosition3DZ() << ")";
    ret = Math::rotateAroundZAxis(ret, -phi);
  }

  return ret;

}

} // namespace VWM
} // namespace BuildingControlUnit