/*
 * Copyright 2010 SRI International
 *
 * This program 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 3 of the License, or
 * (at your option) any later version.
 *
 * This program 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 program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <math.h>
#include <assert.h>
#include <float.h>
#include <limits.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>

#include "open_karto/Karto.h"

namespace karto
{

  ////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////

  SensorManager* SensorManager::GetInstance()
  {
    static Singleton<SensorManager> sInstance;
    return sInstance.Get();
  }

  ////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////

  Object::Object()
    : m_pParameterManager(new ParameterManager())
  {
  }

  Object::Object(const Name& rName)
    : m_Name(rName)
    , m_pParameterManager(new ParameterManager())
  {
  }

  Object::~Object()
  {
    delete m_pParameterManager;
    m_pParameterManager = NULL;
  }

  ////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////

  Module::Module(const std::string& rName)
    : Object(rName)
  {
  }

  Module::~Module()
  {
  }

  ////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////

  Sensor::Sensor(const Name& rName)
    : Object(rName)
  {
    m_pOffsetPose = new Parameter<Pose2>("OffsetPose", Pose2(), GetParameterManager());
  }

  Sensor::~Sensor()
  {
  }

  ////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////

  SensorData::SensorData(const Name& rSensorName)
    : Object()
    , m_StateId(-1)
    , m_UniqueId(-1)
    , m_SensorName(rSensorName)
    , m_Time(0.0)
  {
  }

  SensorData::~SensorData()
  {
    forEach(CustomDataVector, &m_CustomData)
    {
      delete *iter;
    }

    m_CustomData.clear();
  }

  ////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////

  void CellUpdater::operator() (kt_int32u index)
  {
    kt_int8u* pDataPtr = m_pOccupancyGrid->GetDataPointer();
    kt_int32u* pCellPassCntPtr = m_pOccupancyGrid->m_pCellPassCnt->GetDataPointer();
    kt_int32u* pCellHitCntPtr = m_pOccupancyGrid->m_pCellHitsCnt->GetDataPointer();

    m_pOccupancyGrid->UpdateCell(&pDataPtr[index], pCellPassCntPtr[index], pCellHitCntPtr[index]);
  }

  ////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////

  std::ostream& operator << (std::ostream& rStream, Exception& rException)
  {
    rStream << "Error detect: " << std::endl;
    rStream << " ==> error code: " << rException.GetErrorCode() << std::endl;
    rStream << " ==> error message: " << rException.GetErrorMessage() << std::endl;

    return rStream;
  }

  ////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////

  const PointVectorDouble LaserRangeFinder::GetPointReadings(LocalizedRangeScan* pLocalizedRangeScan,
                                                             CoordinateConverter* pCoordinateConverter,
                                                             kt_bool ignoreThresholdPoints,
                                                             kt_bool flipY) const
  {
    PointVectorDouble pointReadings;

    Pose2 scanPose = pLocalizedRangeScan->GetSensorPose();

    // compute point readings
    kt_int32u beamNum = 0;
    kt_double* pRangeReadings = pLocalizedRangeScan->GetRangeReadings();
    for (kt_int32u i = 0; i < m_NumberOfRangeReadings; i++, beamNum++)
    {
      kt_double rangeReading = pRangeReadings[i];

      if (ignoreThresholdPoints)
      {
        if (!math::InRange(rangeReading, GetMinimumRange(), GetRangeThreshold()))
        {
          continue;
        }
      }
      else
      {
        rangeReading = math::Clip(rangeReading, GetMinimumRange(), GetRangeThreshold());
      }

      kt_double angle = scanPose.GetHeading() + GetMinimumAngle() + beamNum * GetAngularResolution();

      Vector2<kt_double> point;

      point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
      point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));

      if (pCoordinateConverter != NULL)
      {
        Vector2<kt_int32s> gridPoint = pCoordinateConverter->WorldToGrid(point, flipY);
        point.SetX(gridPoint.GetX());
        point.SetY(gridPoint.GetY());
      }

      pointReadings.push_back(point);
    }

    return pointReadings;
  }

  kt_bool LaserRangeFinder::Validate(SensorData* pSensorData)
  {
    LaserRangeScan* pLaserRangeScan = dynamic_cast<LaserRangeScan*>(pSensorData);

    // verify number of range readings in LaserRangeScan matches the number of expected range readings
    // if (pLaserRangeScan->GetNumberOfRangeReadings() != GetNumberOfRangeReadings())
    // {
    //   std::cout << "LaserRangeScan contains " << pLaserRangeScan->GetNumberOfRangeReadings()
    //             << " range readings, expected " << GetNumberOfRangeReadings() << std::endl;
    //   return false;
    // }

    return true;
  }

  ////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////

  void ParameterManager::Clear()
  {
    forEach(karto::ParameterVector, &m_Parameters)
    {
      delete *iter;
    }

    m_Parameters.clear();

    m_ParameterLookup.clear();
  }

  void ParameterManager::Add(AbstractParameter* pParameter)
  {
    if (pParameter != NULL && pParameter->GetName() != "")
    {
      if (m_ParameterLookup.find(pParameter->GetName()) == m_ParameterLookup.end())
      {
        m_Parameters.push_back(pParameter);

        m_ParameterLookup[pParameter->GetName()] = pParameter;
      }
      else
      {
        m_ParameterLookup[pParameter->GetName()]->SetValueFromString(pParameter->GetValueAsString());

        assert(false);
      }
    }
  }

  ////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////

  /*
  std::string LaserRangeFinder::LaserRangeFinderTypeNames[6] =
  {
    "Custom",

    "Sick_LMS100",
    "Sick_LMS200",
    "Sick_LMS291",

    "Hokuyo_UTM_30LX",
    "Hokuyo_URG_04LX"
  };
   */
}  // namespace karto