//Copyright (c) 2013 Singapore-MIT Alliance for Research and Technology
//Licensed under the terms of the MIT License, as described in the file:
//   license.txt   (http://opensource.org/licenses/MIT)

#include <algorithm>
#include <boost/utility.hpp>

#include "LoopDetectorEntity.hpp"

#include "conf/ConfigManager.hpp"
#include "conf/ConfigParams.hpp"
#include "config/ST_Config.hpp"
#include "entities/AuraManager.hpp"
#include "Person_ST.hpp"
#include "entities/roles/driver/Driver.hpp"
#include "entities/roles/Role.hpp"
#include "entities/Sensor.hpp"
#include "entities/signal/Signal.hpp"
#include "geospatial/network/Node.hpp"
#include "geospatial/network/Lane.hpp"
#include "geospatial/network/Link.hpp"
#include "geospatial/network/RoadNetwork.hpp"
#include "geospatial/network/RoadSegment.hpp"
#include "logging/Log.hpp"

using namespace sim_mob;
using std::vector;
typedef sim_mob::Entity::UpdateStatus UpdateStatus;

namespace
{
Point zero(0, 0);

bool isNotInitialized(AABB const & aabb)
{
    return (aabb.lowerLeft_.getX() == zero.getX() && aabb.lowerLeft_.getY() == zero.getY()
            && aabb.upperRight_.getX() == zero.getX() && aabb.upperRight_.getY() == zero.getY());
}

Box getBox(const Point& p1, const Point& p2, double halfWidth,
        double length, double offset, int id)
{
    Box box;

    Vector2D<double> lastLine = normalize(Vector2D<double>(p2.getX() - p1.getX(), p2.getY() - p1.getY()));
    Vector2D<double> ldHorizontal = Vector2D<double>(-lastLine.getY(), lastLine.getX());

    Vector2D<double> moveRight = ldHorizontal * (halfWidth);
    Vector2D<double> moveLeft = ldHorizontal * (-halfWidth);

    Vector2D<double> bottomCenter = lastLine * (-(length + offset));
    Vector2D<double> topCenter = lastLine * (-offset);

    Point bottom = Point(p2.getX() + bottomCenter.getX(), p2.getY() + bottomCenter.getY());
    Point top = Point(p2.getX() + topCenter.getX(), p2.getY() + topCenter.getY());

    box.upperLeft_ = Point(top.getX()+moveLeft.getX(), top.getY()+moveLeft.getY());
    box.upperRight_ = Point(top.getX()+moveRight.getX(), top.getY()+moveRight.getY());
    box.lowerLeft_ = Point(bottom.getX()+moveLeft.getX(), bottom.getY()+moveLeft.getY());
    box.lowerRight_ = Point(bottom.getX()+moveRight.getX(), bottom.getY()+moveRight.getY());

    return box;
}
}

/** \cond ignoreLoopDetectorEntityInnards -- Start of block to be ignored by doxygen.  */

void AABB::united(AABB const &another)
{
    int left;
    int right;
    int bottom;
    int top;

    if (this->lowerLeft_.getX() < another.lowerLeft_.getX())
        left = this->lowerLeft_.getX();
    else
        left = another.lowerLeft_.getX();
    if (this->lowerLeft_.getY() < another.lowerLeft_.getY())
        bottom = this->lowerLeft_.getY();
    else
        bottom = another.lowerLeft_.getY();
    if (this->upperRight_.getX() > another.upperRight_.getX())
        right = this->upperRight_.getX();
    else
        right = another.upperRight_.getX();
    if (this->upperRight_.getY() > another.upperRight_.getY())
        top = this->upperRight_.getY();
    else
        top = another.upperRight_.getY();

    this->lowerLeft_ = Point(left, bottom);
    this->upperRight_ = Point(right, top);
}

LoopDetector::LoopDetector(Lane const *lane, meter_t length, Shared<Sensor::CountAndTimePair> &pair)
: width_(lane->getWidth())
, request_to_reset_(false)
, countAndTimePair_(pair)
{
    const std::vector<PolyPoint> polyline = lane->getPolyLine()->getPoints();
    size_t count = polyline.size();

    // The last line of the lane's poly-line is from <p1> to <p2>.
    Point const & p1 = polyline[count - 2];
    Point const & p2 = polyline[count - 1];

    ld_area = getBox(p1, p2, width_/2.5, length, 0.5, lane->getLaneId());

    leftEdge = Vector2D<double>(ld_area.upperLeft_.getX() - ld_area.lowerLeft_.getX(),
            ld_area.upperLeft_.getY() - ld_area.lowerLeft_.getY());
    bottomEdge = Vector2D<double>(ld_area.lowerRight_.getX() - ld_area.lowerLeft_.getX(),
            ld_area.lowerRight_.getY() - ld_area.lowerLeft_.getY());

    sqLenOfLeftEdge = ((leftEdge.getX() * leftEdge.getX()) + (leftEdge.getY() * leftEdge.getY()));
    sqLenOfBottomEdge = ((bottomEdge.getX() * bottomEdge.getX()) + (bottomEdge.getY() * bottomEdge.getY()));

    timeStepInMilliSeconds_ = ST_Config::getInstance().personTimeStepInMilliSeconds();
}

Box LoopDetector::getLDBox() const
{
    return ld_area;
}

bool LoopDetector::check(boost::unordered_set<Vehicle const *> &vehicles, std::vector<Vehicle const *>& vehsInLoopDetector)
{
    // In the previous version, LoopDetectorEntity::reset() was allowed to modify countAndTimePair_.
    // Therefore, countAndTimePair_ could be modified via 2 execution paths -- reset() and this
    // method, check().  This causes a race condition because reset() is called by the Signal
    // object on one thread and check() is called by the LoopDetectorEntity object on another
    // thread.  As a result, sometimes the countAndTimePair_ did not get reset.
    //
    // To prevent race conditions, there can only be 1 writer for any variable.  Therefore, we
    // make check() the only writer of countAndTimePair_.  The LoopDetectorEntity::reset() now
    // "requests" for countAndTimePair_ to be reset.  Now there is a race condition on the
    // request_to_reset_ variable, but this race will not cause any issue except that
    // countAndTimePair_ may get reset twice in a row.  That is not serious.
    if (request_to_reset_)
    {
        request_to_reset_ = false;
        countAndTimePair_.force(Sensor::CountAndTimePair());
        return false;
    }

    boost::unordered_set<Vehicle const *>::iterator iter;
    for (iter = vehicles.begin(); iter != vehicles.end(); ++iter)
    {
        Vehicle const *vehicle = *iter;
        if (check(*vehicle))
        {
            if (std::find(vehicles_.begin(), vehicles_.end(), vehicle) == vehicles_.end())
            {
                vehicles_.push_back(vehicle);
                vehsInLoopDetector.push_back(vehicle);
                incrementVehicleCount();
            }

            // Remove this vehicle from the <vehicles> list so that the other loop detectors
            // do not have to bother with this vehicle, which cannot be hovering over them as well.
            vehicles.erase(iter);

            // Return immediately even if there is another vehicle over the loop detector.
            // Eventually one will move away.
            return true;
        }
        else
        {
            std::vector<const Vehicle*>::iterator iter = std::find(vehicles_.begin(), vehicles_.end(), vehicle);
            if(iter != vehicles_.end())
            {
                vehicles_.erase(iter);
            }
        }
    }

    // No vehicle (or part of) is hovering over the loop detector.
    incrementSpaceTime();
    return false;
}

bool LoopDetector::check(Vehicle const & vehicle)
{
    Point vehPos = vehicle.getCurrPosition();

    // Line connecting the lower left corner of loop detector and vehicle position
    Vector2D<double> line(vehPos.getX() - ld_area.lowerLeft_.getX(),
            vehPos.getY() - ld_area.lowerLeft_.getY());

    double projOnLeftEdge = (line * leftEdge);
    if (0 <= projOnLeftEdge && projOnLeftEdge <= sqLenOfLeftEdge)
    {
        double projOnBottomEdge = (line * bottomEdge);
        if (0 <= projOnBottomEdge && projOnBottomEdge <= sqLenOfBottomEdge)
        {
            return true;
        }
    }

    return false;
}

////////////////////////////////////////////////////////////////////////////////////////////
// LoopDetectorEntity::Impl
////////////////////////////////////////////////////////////////////////////////////////////

class LoopDetectorEntity::Impl : private boost::noncopyable
{
public:
    Impl(Signal const &signal, LoopDetectorEntity &entity);
    ~Impl();

    // Check if any vehicle is hovering over any of the loop detectors or not.
    bool check(timeslice now);

    // Called by the Signal object at the end of its cycle to reset all CountAndTimePair.
    void reset();

    // Called by the Signal object at the end of its cycle to reset the CountAndTimePair
    // for the specified <lane>.
    void reset(Lane const &lane);

private:
    meter_t length;

    // Collection of loop detectors managed by this entity.
    std::map<Lane const *, LoopDetector *> loopDetectors_;

    // The smallest rectangle that would contain all of <loopDetectors_>.  Since the loop detectors
    // could be on opposite sides of the intersection, this area would include the intersection.
    // Alternatively, the LoopDetectorEntity could be implemented as having a collection of
    // disjoint monitor areas.  In that case, the check() would make calls to the AuraManager for
    // each of the monitor areas.
    AABB monitorArea_;

    //For reference.
    const LoopDetectorEntity *parent;

private:
    void
    createLoopDetectors(Signal const &signal, LoopDetectorEntity &entity);

    void
    createLoopDetectors(std::vector<RoadSegment *> const &roads, LoopDetectorEntity &entity);

    void computeMonitorArea();

    BasicLogger& assignmentMatrixLogger;
    ST_Config& stCfg;
};

LoopDetectorEntity::Impl::Impl(Signal const &signal, LoopDetectorEntity &entity) : parent(&entity),
        stCfg(ST_Config::getInstance()),
        assignmentMatrixLogger(Logger::log(ST_Config::getInstance().outputStats.assignmentMatrix.fileName))
{
    // Assume that each loop-detector is 4 meters in length.  This will be the inner monitoring
    // area.  Any vehicle whose (central) position is within this area will be considered to be
    // hovering over the loop detector.
    length = 4;

    createLoopDetectors(signal, entity);
}

LoopDetectorEntity::Impl::~Impl()
{
    std::map<Lane const *, LoopDetector *>::iterator iter;
    for (iter = loopDetectors_.begin(); iter != loopDetectors_.end(); ++iter)
    {
        LoopDetector * detector = iter->second;
        delete detector;
    }
}

void LoopDetectorEntity::Impl::createLoopDetectors(Signal const &signal, LoopDetectorEntity &entity)
{
    const std::vector<const Node *> &nodes = signal.getNodes();
    std::vector<const Node *>::const_iterator itNodes = nodes.begin();

    const RoadNetwork *network = RoadNetwork::getInstance();
    const std::map<unsigned int, Link *> &links = network->getMapOfIdVsLinks();

    //Iterate over the nodes that are associated with the signal
    while(itNodes != nodes.end())
    {
        const std::map<unsigned int, std::map<unsigned int, TurningGroup *> >& turningsFromMap = (*itNodes)->getTurningGroups();
        std::map<unsigned int, std::map<unsigned int, TurningGroup *> >::const_iterator itTurningsFromMap = turningsFromMap.begin();

        //Iterate through the map of turning groups at the node
        while (itTurningsFromMap != turningsFromMap.end())
        {
            //Get the incoming links
            const Link *link = network->getById(links, itTurningsFromMap->first);

            //Create loop detector at the last segment in the link
            createLoopDetectors(link->getRoadSegments(), entity);

            ++itTurningsFromMap;
        }

        ++itNodes;
    }
    computeMonitorArea();
}

void LoopDetectorEntity::Impl::computeMonitorArea()
{
    if(loopDetectors_.empty())
    {
        return;
    }

    // TODO: Currently computing the rectangle which contains all the loop detector
    //       in a signalized intersection. To improve the performance minimum bounding
    //       rectangle should be computed

    std::vector<Point> vecPoints;
    for (auto item : loopDetectors_)
    {
        Box box = item.second->getLDBox();
        vecPoints.push_back(box.lowerLeft_);
        vecPoints.push_back(box.upperLeft_);
        vecPoints.push_back(box.upperRight_);
        vecPoints.push_back(box.lowerRight_);
    }

    auto xExtremes = std::minmax_element(vecPoints.begin(), vecPoints.end(),
                    [](const Point& lhs, const Point& rhs) {
                            return lhs.getX() < rhs.getX();
                     });

    auto yExtremes = std::minmax_element(vecPoints.begin(), vecPoints.end(),
                        [](const Point& lhs, const Point& rhs) {
                                return lhs.getY() < rhs.getY();
                         });

    monitorArea_.lowerLeft_ = Point((*xExtremes.first).getX(), (*yExtremes.first).getY());
    monitorArea_.upperRight_ = Point((*xExtremes.second).getX(), (*yExtremes.second).getY());
}

void LoopDetectorEntity::Impl::createLoopDetectors(std::vector<RoadSegment *> const & roads, LoopDetectorEntity & entity)
{
    size_t count = roads.size();
    size_t createdLDs = 0;
    RoadSegment const * road = roads[count - 1]; //create LD only for the last road segment in the vector
    std::vector<const Lane *> const & lanes = road->getLanes();

    if (!lanes.size())
    {
        std::ostringstream str;
        str << __func__ << ": There is no lane associated with road segment " << road->getRoadSegmentId();
        throw std::runtime_error(str.str());
    }

    for (size_t i = 0; i < lanes.size(); ++i)
    {
        Lane const * lane = lanes[i];
        if (lane->isPedestrianLane())
        {
            continue;
        }

        //Shared<CountAndTimePair> & pair = entity.data_[lane];
        std::map<Lane const *, Shared<CountAndTimePair> *>::iterator iter = entity.data.find(lane);
        if (entity.data.end() == iter)
        {
            MutexStrategy const & mutexStrategy = ConfigManager::GetInstance().FullConfig().mutexStategy();
            Shared<CountAndTimePair> * pair = new Shared<CountAndTimePair>(mutexStrategy);
            iter = entity.data.insert(std::make_pair(lane, pair)).first;
        }
        Shared<CountAndTimePair> * pair = iter->second;

        LoopDetector* detector = new LoopDetector(lane, length, *pair);
        loopDetectors_.insert(std::make_pair(lane, detector));

        createdLDs++;
    }

    if (createdLDs == 0)
    {
        std::ostringstream str;
        str << __func__ << ": Could not create any loop detector in road segment " << road->getRoadSegmentId()
            << "\nNote: laneDS cannot be calculated...";
        throw std::runtime_error(str.str());
    }
}

bool LoopDetectorEntity::Impl::check(timeslice now)
{
    // Get all vehicles located within monitorArea_.
    boost::unordered_set<Vehicle const *> vehicles;
    boost::unordered_map<Vehicle const *, Person_ST const *> vehicleToPersonMap;
    std::vector<Agent const *> const agents = AuraManager::instance().agentsInRect(monitorArea_.lowerLeft_, monitorArea_.upperRight_, parent);
    for (size_t i = 0; i < agents.size(); ++i)
    {
        Agent const * agent = agents[i];
        if (Person_ST const * person = dynamic_cast<Person_ST const *> (agent))
        {
            //Extract the current resource used by this Agent.
            const Role<Person_ST> *role = person->getRole();

            if(role)
            {
                const Driver* driver = dynamic_cast<const Driver*>(role);

                if(!driver || loopDetectors_.find(driver->getCurrLane()) == loopDetectors_.end())
                {
                    continue;
                }

                const Vehicle * vehicle = dynamic_cast<Vehicle *> (role->getResource());

                if (vehicle)
                {
                    vehicles.insert(vehicle);
                    vehicleToPersonMap[vehicle] = person;
                }
            }
        }
    }

    // Pass the vehicles list to the loop detectors.
    std::map<Lane const *, LoopDetector *>::const_iterator iter;
    for (iter = loopDetectors_.begin(); iter != loopDetectors_.end(); ++iter)
    {
        std::vector<Vehicle const *> vehsInLoopDetector;
        iter->second->check(vehicles, vehsInLoopDetector);

        if(stCfg.outputStats.assignmentMatrix.enabled)
        {
            for (std::vector<Vehicle const *>::const_iterator vehIter = vehsInLoopDetector.begin();
                    vehIter != vehsInLoopDetector.end(); vehIter++)
            {
                Person_ST const * person = vehicleToPersonMap[(*vehIter)];

                if(person->getRole()->roleType != sim_mob::Role<Person_ST>::RL_DRIVER)
                {
                    continue;
                }

                assignmentMatrixLogger << iter->first->getParentSegment()->getParentLink()->getToNode()->getTrafficLightId()
                        << "," << iter->first->getParentSegment()->getRoadSegmentId()
                        << "," << iter->first->getLaneId()
                        << "," << now.ms()
                        << "," << (person->getDatabaseId().empty() ? "-1" : person->getDatabaseId())
                        << "," << (*person->currSubTrip).origin.node->getNodeId()
                        << "," << (*person->currSubTrip).destination.node->getNodeId()
                        << "," <<(*person->currSubTrip).startTime.getValue() << "\n" ;
            }
        }
    }

    return true;
}

void LoopDetectorEntity::Impl::reset()
{
    std::map<Lane const *, LoopDetector *>::const_iterator iter;
    for (iter = loopDetectors_.begin(); iter != loopDetectors_.end(); ++iter)
    {
        LoopDetector * detector = iter->second;
        detector->reset();
    }
}

void LoopDetectorEntity::Impl::reset(Lane const &lane)
{
    std::map<Lane const *, LoopDetector *>::const_iterator iter = loopDetectors_.find(&lane);

    if (iter != loopDetectors_.end())
    {
        LoopDetector * detector = iter->second;
        detector->reset();
    }
    else
    {
        std::stringstream stream;
        stream << __func__ << ": Lane does not contain Loop Detectors";
        throw std::runtime_error(stream.str());
    }
}

/** \endcond ignoreLoopDetectorInnards -- End of block to be ignored by doxygen.  */

////////////////////////////////////////////////////////////////////////////////////////////
// LoopDetectorEntity
////////////////////////////////////////////////////////////////////////////////////////////

LoopDetectorEntity::~LoopDetectorEntity()
{
    if (pimpl_)
    {
        delete pimpl_;
    }
}

void LoopDetectorEntity::init(Signal const &signal)
{
    pimpl_ = new Impl(signal, *this);
}

Entity::UpdateStatus LoopDetectorEntity::frame_tick(timeslice now)
{
    if (pimpl_)
    {
        return pimpl_->check(now) ? UpdateStatus::Continue : UpdateStatus::Done;
    }
    return UpdateStatus::Done;
}

void LoopDetectorEntity::reset()
{
    if (pimpl_)
        pimpl_->reset();
}

void LoopDetectorEntity::reset(Lane const &lane)
{
    if (pimpl_)
        pimpl_->reset(lane);
}


/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 The SKA organisation
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

// This class taken from the orignal ska::cheetah::units class

#include "../TimePoint.h"
#include "../BoostDurationCast.h"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wall"
#pragma GCC diagnostic ignored "-Wpragmas"
#pragma GCC diagnostic ignored "-Wunused-parameter"
#pragma GCC diagnostic ignored "-Wunused-variable"
#include <boost/units/quantity.hpp>
#include <boost/units/conversion.hpp>
#include <boost/units/systems/si/time.hpp>
#pragma GCC diagnostic pop
#include <iomanip>
#include <ctime>

namespace pss {
namespace astrotypes {
namespace units {

template<typename ClockType, typename Duration>
TimePoint<ClockType, Duration>::TimePoint( const Duration& d )
    : std::chrono::time_point<ClockType, Duration>(d)
{
}

template<typename ClockType, typename Duration>
template<typename QuantityType>
TimePoint<ClockType, Duration>::TimePoint( QuantityType const& d, typename std::enable_if<
                               boost::units::is_quantity<QuantityType>::value
                            && boost::units::is_unit_of_dimension<typename QuantityType::unit_type, boost::units::time_dimension>::value
                      , QuantityType>::type)
    : std::chrono::time_point<ClockType, Duration>(duration_cast<typename ClockType::duration>(d))
{
}

template<typename ClockType, typename Duration>
TimePoint<ClockType, Duration>::TimePoint( const std::chrono::system_clock::time_point& tp )
    : std::chrono::time_point<ClockType, Duration>(std::chrono::duration_cast<typename ClockType::duration>(tp.time_since_epoch()) + ClockType::diff_from_system_epoch)
{
}

template<typename ClockType, typename Duration>
TimePoint<ClockType, Duration>::TimePoint( const std::chrono::time_point<ClockType, Duration>& tp )
    : std::chrono::time_point<ClockType, Duration>(tp)
{
}

template<typename ClockType, typename Duration>
TimePoint<ClockType, Duration>::TimePoint( std::chrono::time_point<ClockType, Duration>&& tp )
    : std::chrono::time_point<ClockType, Duration>(std::move(tp))
{
}

template<typename ClockType, typename Duration>
TimePoint<ClockType, Duration>::operator typename std::chrono::system_clock::time_point() const
{
    auto dur = this->time_since_epoch() - ClockType::diff_from_system_epoch;
    return std::chrono::system_clock::time_point(std::chrono::duration_cast<std::chrono::system_clock::duration>(dur));
}

template<typename ClockType, typename Duration>
std::time_t TimePoint<ClockType, Duration>::to_time_t() const
{
    return std::chrono::system_clock::to_time_t(static_cast<std::chrono::system_clock::time_point>(*this));
}

template<typename ClockType, typename Duration>
std::ostream& operator<<(std::ostream& os, TimePoint<ClockType, Duration> const& tp)
{
    //std::time_t tt = tp.to_time_t();
    //os << std::put_time(std::gmtime(&tt), "%F %T"); // not implemented till gcc 5.0
    //os << asctime(std::gmtime(&tt));
    os << tp.time_since_epoch().count() << " " << ClockType::symbol;
    return os;
}

template<typename ClockType, typename Duration>
TimePoint<ClockType, Duration>& TimePoint<ClockType, Duration>::operator+=(Duration const& d)
{
    std::chrono::time_point<ClockType, Duration>::operator+=(d);
    return *this;
}

template<typename ClockType, typename Duration>
TimePoint<ClockType, Duration>& TimePoint<ClockType, Duration>::operator-=(Duration const& d)
{
    std::chrono::time_point<ClockType, Duration>::operator-=(d);
    return *this;
}

template<typename ClockType, typename Duration, typename Duration2>
TimePoint<ClockType, Duration> operator+(TimePoint<ClockType, Duration> const& lhs, Duration2 const& rhs) {
    return TimePoint<ClockType, Duration>(static_cast<std::chrono::time_point<ClockType, Duration>>(lhs) + rhs);
}

template<typename ClockType, typename DurationType, typename Rep>
TimePoint<ClockType, DurationType> operator+(TimePoint<ClockType, DurationType> const& lhs, boost::units::quantity<boost::units::si::time, Rep> const& rhs) {
    typedef typename ClockType::duration Duration;
    return lhs + duration_cast<Duration>(rhs);
}

template<typename ClockType, typename DurationType, typename Rep>
TimePoint<ClockType, DurationType>& operator+=(TimePoint<ClockType, DurationType>& lhs, boost::units::quantity<boost::units::si::time, Rep> const& rhs)
{
    typedef typename ClockType::duration Duration;
    return lhs += duration_cast<Duration>(rhs);
}

template<typename ClockType, typename DurationType, typename Rep>
TimePoint<ClockType, DurationType> operator-(TimePoint<ClockType, DurationType> const& lhs, boost::units::quantity<boost::units::si::time, Rep> const& rhs) {
    typedef typename ClockType::duration Duration;
    return lhs - duration_cast<Duration>(rhs);
}

template<typename ClockType, typename DurationType, typename Rep>
TimePoint<ClockType, DurationType>& operator-=(TimePoint<ClockType, DurationType>& lhs, boost::units::quantity<boost::units::si::time, Rep> const& rhs)
{
    typedef typename ClockType::duration Duration;
    return lhs -= duration_cast<Duration>(rhs);
}

template<typename ClockType, typename Duration, typename Duration2>
TimePoint<ClockType, Duration> operator-(TimePoint<ClockType, Duration> const& lhs, Duration2 const& rhs) {
    return TimePoint<ClockType, Duration>(static_cast<std::chrono::time_point<ClockType, Duration>>(lhs) - rhs);
}

template<typename ClockType, typename Duration, typename Duration2>
Duration operator-(TimePoint<ClockType, Duration> const& lhs, TimePoint<ClockType, Duration2> const& rhs)
{
    return static_cast<std::chrono::time_point<ClockType, Duration>>(lhs) - static_cast<std::chrono::time_point<ClockType, Duration2>>(rhs);
}

} // namespace units
} // namespace astrotypes
} // namespace pss


/*******************************************************************************
 *
 * Generic implementation of non-relational domains.
 *
 * Author: Arnaud J. Venet (arnaud.j.venet@nasa.gov)
 *
 * Notices:
 *
 * Copyright (c) 2011 United States Government as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All Rights Reserved.
 *
 * Disclaimers:
 *
 * No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF
 * ANY KIND, EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED
 * TO, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS,
 * ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE,
 * OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL BE
 * ERROR FREE, OR ANY WARRANTY THAT DOCUMENTATION, IF PROVIDED, WILL CONFORM TO
 * THE SUBJECT SOFTWARE. THIS AGREEMENT DOES NOT, IN ANY MANNER, CONSTITUTE AN
 * ENDORSEMENT BY GOVERNMENT AGENCY OR ANY PRIOR RECIPIENT OF ANY RESULTS,
 * RESULTING DESIGNS, HARDWARE, SOFTWARE PRODUCTS OR ANY OTHER APPLICATIONS
 * RESULTING FROM USE OF THE SUBJECT SOFTWARE.  FURTHER, GOVERNMENT AGENCY
 * DISCLAIMS ALL WARRANTIES AND LIABILITIES REGARDING THIRD-PARTY SOFTWARE,
 * IF PRESENT IN THE ORIGINAL SOFTWARE, AND DISTRIBUTES IT "AS IS."
 *
 * Waiver and Indemnity:  RECIPIENT AGREES TO WAIVE ANY AND ALL CLAIMS AGAINST
 * THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS, AS WELL
 * AS ANY PRIOR RECIPIENT.  IF RECIPIENT'S USE OF THE SUBJECT SOFTWARE RESULTS
 * IN ANY LIABILITIES, DEMANDS, DAMAGES, EXPENSES OR LOSSES ARISING FROM SUCH
 * USE, INCLUDING ANY DAMAGES FROM PRODUCTS BASED ON, OR RESULTING FROM,
 * RECIPIENT'S USE OF THE SUBJECT SOFTWARE, RECIPIENT SHALL INDEMNIFY AND HOLD
 * HARMLESS THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS,
 * AS WELL AS ANY PRIOR RECIPIENT, TO THE EXTENT PERMITTED BY LAW.
 * RECIPIENT'S SOLE REMEDY FOR ANY SUCH MATTER SHALL BE THE IMMEDIATE,
 * UNILATERAL TERMINATION OF THIS AGREEMENT.
 *
 ******************************************************************************/

#pragma once

#include <crab/domains/patricia_trees.hpp>
#include  <crab/domains/discrete_domains.hpp>
#include <crab/support/debug.hpp>

#include <boost/optional.hpp>

#include <algorithm>
#include <set>
#include <vector>

namespace ikos {

/* Environment from Key to Value with all lattice operations */
template <typename Key, typename Value> class separate_domain {

private:
  using patricia_tree_t = patricia_tree<Key, Value>;
  using unary_op_t = typename patricia_tree_t::unary_op_t;
  using binary_op_t = typename patricia_tree_t::binary_op_t;
  using partial_order_t = typename patricia_tree_t::partial_order_t;

public:
  using separate_domain_t = separate_domain<Key, Value>;
  using iterator = typename patricia_tree_t::iterator;
  using key_type = Key;
  using value_type = Value;

private:
  bool _is_bottom;
  patricia_tree_t _tree;

public:
  class join_op : public binary_op_t {
    std::pair<bool, boost::optional<Value>> apply(Value x, Value y) {
      Value z = x.operator|(y);
      if (z.is_top()) {
        return {false, boost::optional<Value>()};
      } else {
        return {false, boost::optional<Value>(z)};
      }
    };

    bool default_is_absorbing() { return true; }
  }; // class join_op

  class widening_op : public binary_op_t {
    std::pair<bool, boost::optional<Value>> apply(Value x, Value y) {
      Value z = x.operator||(y);
      if (z.is_top()) {
        return {false, boost::optional<Value>()};
      } else {
        return {false, boost::optional<Value>(z)};
      }
    };

    bool default_is_absorbing() { return true; }

  }; // class widening_op

  template <typename Thresholds>
  class widening_thresholds_op : public binary_op_t {
    const Thresholds &m_ts;

  public:
    widening_thresholds_op(const Thresholds &ts) : m_ts(ts) {}

    std::pair<bool, boost::optional<Value>> apply(Value x, Value y) {
      Value z = x.widening_thresholds(y, m_ts);
      if (z.is_top()) {
        return {false, boost::optional<Value>()};
      } else {
        return {false, boost::optional<Value>(z)};
      }
    };

    bool default_is_absorbing() { return true; }

  }; // class widening_thresholds_op

  class meet_op : public binary_op_t {
    std::pair<bool, boost::optional<Value>> apply(Value x, Value y) {
      Value z = x.operator&(y);
      if (z.is_bottom()) {
        return {true, boost::optional<Value>()};
      } else {
        return {false, boost::optional<Value>(z)};
      }
    };

    bool default_is_absorbing() { return false; }

  }; // class meet_op

  class narrowing_op : public binary_op_t {
    std::pair<bool, boost::optional<Value>> apply(Value x, Value y) {
      Value z = x.operator&&(y);
      if (z.is_bottom()) {
        return {true, boost::optional<Value>()};
      } else {
        return {false, boost::optional<Value>(z)};
      }
    };

    bool default_is_absorbing() { return false; }

  }; // class narrowing_op

  class domain_po : public partial_order_t {
    bool leq(Value x, Value y) { return x.operator<=(y); }

    bool default_is_top() { return true; }

  }; // class domain_po

public:
  static separate_domain_t top() { return separate_domain_t(); }

  static separate_domain_t bottom() { return separate_domain_t(false); }

private:
  static patricia_tree_t apply_operation(binary_op_t &o, patricia_tree_t t1,
                                         const patricia_tree_t &t2,
                                         bool &is_bottom) {
    is_bottom = t1.merge_with(t2, o);
    return t1;
  }

  separate_domain(patricia_tree_t t) : _is_bottom(false), _tree(t) {}

  separate_domain(bool b) : _is_bottom(!b) {}

public:
  separate_domain() : _is_bottom(false) {}

  separate_domain(const separate_domain_t &e)
      : _is_bottom(e._is_bottom), _tree(e._tree) {}

  separate_domain(const separate_domain_t &&e)
      : _is_bottom(e._is_bottom), _tree(std::move(e._tree)) {}

  separate_domain_t &operator=(separate_domain_t e) {
    this->_is_bottom = e._is_bottom;
    this->_tree = e._tree;
    return *this;
  }

  iterator begin() const {
    if (this->is_bottom()) {
      CRAB_ERROR("Separate domain: trying to invoke iterator on bottom");
    } else {
      return this->_tree.begin();
    }
  }

  iterator end() const {
    if (this->is_bottom()) {
      CRAB_ERROR("Separate domain: trying to invoke iterator on bottom");
    } else {
      return this->_tree.end();
    }
  }

  bool is_bottom() const { return this->_is_bottom; }

  bool is_top() const {
    return (!this->is_bottom() && this->_tree.size() == 0);
  }

  bool operator<=(const separate_domain_t &e) const {
    if (this->is_bottom()) {
      return true;
    } else if (e.is_bottom()) {
      return false;
    } else {
      domain_po po;
      return this->_tree.leq(e._tree, po);
    }
  }

  bool operator==(const separate_domain_t &e) const {
    return (this->operator<=(e) && e.operator<=(*this));
  }

  // Join
  separate_domain_t operator|(const separate_domain_t &e) const {
    if (this->is_bottom()) {
      return e;
    } else if (e.is_bottom()) {
      return *this;
    } else {
      join_op o;
      bool is_bottom /*unused*/;
      patricia_tree_t res = apply_operation(o, this->_tree, e._tree, is_bottom);
      return separate_domain_t(std::move(res));
    }
  }

  // Meet
  separate_domain_t operator&(const separate_domain_t &e) const {
    if (this->is_bottom() || e.is_bottom()) {
      return this->bottom();
    } else {
      meet_op o;
      bool is_bottom;
      patricia_tree_t res = apply_operation(o, this->_tree, e._tree, is_bottom);
      if (is_bottom) {
        return this->bottom();
      } else {
        return separate_domain_t(std::move(res));
      }
    }
  }

  // Widening
  separate_domain_t operator||(const separate_domain_t &e) const {
    if (this->is_bottom()) {
      return e;
    } else if (e.is_bottom()) {
      return *this;
    } else {
      widening_op o;
      bool is_bottom /*unused*/;
      patricia_tree_t res = apply_operation(o, this->_tree, e._tree, is_bottom);
      return separate_domain_t(std::move(res));
    }
  }

  // Widening with thresholds
  template <typename Thresholds>
  separate_domain_t widening_thresholds(const separate_domain_t &e,
                                        const Thresholds &ts) const {
    if (this->is_bottom()) {
      return e;
    } else if (e.is_bottom()) {
      return *this;
    } else {
      widening_thresholds_op<Thresholds> o(ts);
      bool is_bottom /*unused*/;
      patricia_tree_t res = apply_operation(o, this->_tree, e._tree, is_bottom);
      return separate_domain_t(std::move(res));
    }
  }

  // Narrowing
  separate_domain_t operator&&(const separate_domain_t &e) const {
    if (this->is_bottom() || e.is_bottom()) {
      return separate_domain_t(false);
    } else {
      narrowing_op o;
      bool is_bottom;
      patricia_tree_t res = apply_operation(o, this->_tree, e._tree, is_bottom);
      if (is_bottom) {
        return this->bottom();
      } else {
        return separate_domain_t(std::move(res));
      }
    }
  }

  void set(Key k, Value v) {
    if (!this->is_bottom()) {
      if (v.is_bottom()) {
        set_to_bottom();
      } else if (v.is_top()) {
        this->_tree.remove(k);
      } else {
        this->_tree.insert(k, v);
      }
    }
  }

  void set_to_bottom() {
    this->_is_bottom = true;
    this->_tree = patricia_tree_t();
  }

  separate_domain_t &operator-=(const Key &k) {
    if (!this->is_bottom()) {
      this->_tree.remove(k);
    }
    return *this;
  }

  Value operator[](const Key &k) const {
    if (this->is_bottom()) {
      return Value::bottom();
    } else {
      boost::optional<Value> v = this->_tree.lookup(k);
      if (v) {
        return *v;
      } else {
        return Value::top();
      }
    }
  }

  std::size_t size() const {
    if (is_bottom()) {
      return 0;
    } else if (is_top()) {
      CRAB_ERROR("separate_domains::size() is undefined if top");
    } else {
      return this->_tree.size();
    }
  }

  void write(crab::crab_os &o) const {
    if (this->is_bottom()) {
      o << "_|_";
    } else {
      o << "{";
      for (typename patricia_tree_t::iterator it = this->_tree.begin();
           it != this->_tree.end();) {
        Key k = it->first;
        k.write(o);
        o << " -> ";
        Value v = it->second;
        v.write(o);
        ++it;
        if (it != this->_tree.end()) {
          o << "; ";
        }
      }
      o << "}";
    }
  }

  void project(const std::vector<Key> &keys) {
    if (is_bottom() || is_top()) {
      return;
    }
    const int factor = 60;   // between 0 and 100
    const int small_env = 5; // size of a small environment
    int num_total_keys = (int)this->_tree.size();
    int num_keys = (int)keys.size();
    if (num_total_keys <= small_env ||
        num_keys < num_total_keys * factor / 100) {
      // project on less than factor% of keys: we copy
      separate_domain_t env;
      for (auto key : keys) {
        env.set(key, operator[](key));
      }
      std::swap(*this, env);
    } else {
      // project on more or equal then factor% of keys: that
      // might be too many copies so we remove instead.
      std::vector<Key> sorted_keys(keys);
      std::sort(sorted_keys.begin(), sorted_keys.end());
      std::vector<Key> project_out_keys;
      project_out_keys.reserve(num_total_keys);
      for (auto it = this->_tree.begin(); it != this->_tree.end(); ++it) {
        Key key = it->first;
        if (!std::binary_search(sorted_keys.begin(), sorted_keys.end(), key)) {
          project_out_keys.push_back(key);
        }
      }
      for (auto const &key : project_out_keys) {
        this->operator-=(key);
      }
    }
  }

  // Assume that from does not have duplicates.
  void rename(const std::vector<Key> &from, const std::vector<Key> &to) {
    if (is_top() || is_bottom()) {
      // nothing to rename
      return;
    }
    if (from.size() != to.size()) {
      CRAB_ERROR(
          "separate_domain::rename with input vectors of different sizes");
    }

    if (::crab::CrabSanityCheckFlag) {
      std::set<Key> s1, s2;
      s1.insert(from.begin(), from.end());
      if (s1.size() != from.size()) {
        CRAB_ERROR("separate_domain::rename expects no duplicates");
      }
      s2.insert(to.begin(), to.end());
      if (s2.size() != to.size()) {
        CRAB_ERROR("separate_domain::rename expects no duplicates");
      }
    }

    for (unsigned i = 0, sz = from.size(); i < sz; ++i) {
      Key k = from[i];
      Key new_k = to[i];
      if (k == new_k) { // nothing to rename
        continue;
      }
      if (::crab::CrabSanityCheckFlag) {
        if (_tree.lookup(new_k)) {
          CRAB_ERROR("separate_domain::rename assumes that  ", new_k,
                     " does not exist in ", *this);
        }
      }
      if (boost::optional<Value> k_val_opt = _tree.lookup(k)) {
        if (!(*k_val_opt).is_top()) {
          _tree.insert(new_k, *k_val_opt);
        }
        _tree.remove(k);
      }
    }
  }

  friend crab::crab_os &operator<<(crab::crab_os &o,
                                   const separate_domain<Key, Value> &d) {
    d.write(o);
    return o;
  }
}; // class separate_domain

} // namespace ikos

namespace crab {
namespace domains {
//===================================================================//
//        The NOSA license does not apply to this code
//===================================================================//

/*
 * Environment from Key to discrete_domain<Element>
 *
 * We cannot use separate_domain because if the value of a key-value
 * pair is bottom then the whole environment becomes bottom. Here, if
 * the value is bottom then it means that the value is simply the
 * "empty set" so we want to allow entries whose values are
 * bottom. But, similar to separate_domain, the whole environment can
 * still be bottom meaning failure/undefined/unreachable.
 */
template <typename Key, typename Element> class separate_discrete_domain {

private:
  using discrete_domain_t = ikos::discrete_domain<Element>;
  using patricia_tree_t = ikos::patricia_tree<Key, discrete_domain_t>;
  using unary_op_t = typename patricia_tree_t::unary_op_t;
  using binary_op_t = typename patricia_tree_t::binary_op_t;
  using partial_order_t = typename patricia_tree_t::partial_order_t;

public:
  using key_type = Key;
  using value_type = discrete_domain_t;
  using separate_discrete_domain_t = separate_discrete_domain<Key, Element>;
  using iterator = typename patricia_tree_t::iterator;

private:
  bool m_is_bottom;
  patricia_tree_t m_tree;

  static patricia_tree_t apply_operation(binary_op_t &o,
                                         const patricia_tree_t &t1,
                                         const patricia_tree_t &t2) {
    patricia_tree_t res(t1);
    res.merge_with(t2, o);
    return res;
  }
  /* begin patricia_tree API */
  class join_op : public binary_op_t {
    std::pair<bool, boost::optional<value_type>> apply(value_type x, value_type y) override {
      value_type z = x.operator|(y);
      if (z.is_top()) {
	// special encoding for top: the patricia tree will not keep
	// top values.
        return {false, boost::optional<value_type>()};
      } else {
        return {false, boost::optional<value_type>(z)};
      }
    }
    bool default_is_absorbing() override { return true; }
  }; // class join_op

  class meet_op : public binary_op_t {
    std::pair<bool, boost::optional<value_type>> apply(value_type x,  value_type y) override {
      value_type z = x.operator&(y);
      // Returning this pair means that if z is bottom do not treat it
      // special and just update the patricia tree with z.
      return {false, boost::optional<value_type>(z)};
    };
    bool default_is_absorbing() override { return false; }
  }; // class meet_op

  class domain_po : public partial_order_t {
    bool leq(value_type x, value_type y) { return x.operator<=(y); }
    bool default_is_top() { return true; }
  }; // class domain_po
  /* end patricia_tree API */

  separate_discrete_domain(patricia_tree_t &&t)
    : m_is_bottom(false), m_tree(std::move(t)) {}

public:

  static separate_discrete_domain_t top() {
    return separate_discrete_domain_t();
  }

  static separate_discrete_domain_t bottom() {
    return separate_discrete_domain_t(true);
  }

  // Default constructor returns a top environment
  separate_discrete_domain(bool is_bottom = false)
    : m_is_bottom(is_bottom), m_tree(patricia_tree_t()) {}

  separate_discrete_domain(const separate_discrete_domain_t &o) = default;
  separate_discrete_domain(separate_discrete_domain_t &&o) = default;
  separate_discrete_domain_t &operator=(const separate_discrete_domain_t &o)  = default;
  separate_discrete_domain_t &operator=(separate_discrete_domain_t &&o)  = default;

  iterator begin() const {
    if (is_bottom()) {
      CRAB_ERROR("Separate discrete domain: trying to invoke iterator on bottom");
    } else {
      return m_tree.begin();
    }
  }

  iterator end() const {
    if (is_bottom()) {
      CRAB_ERROR("Separate discrete domain: trying to invoke iterator on bottom");
    } else {
      return m_tree.end();
    }
  }

  bool is_bottom() const { return m_is_bottom; }

  bool is_top() const {
    return (!is_bottom()&& m_tree.size() == 0);
  }

  bool operator<=(const separate_discrete_domain_t &other) const {
    if (is_bottom() || other.is_top()) {
      return true;
    } else if (other.is_bottom() || is_top()) {
      return false;
    } else {
      domain_po po;
      return m_tree.leq(other.m_tree, po);
    }
  }

  separate_discrete_domain_t
  operator|(const separate_discrete_domain_t &o) const {
    CRAB_LOG("separate-domain",
	     crab::outs() << "Join " << *this << " and " << o << "=";);

    if (is_bottom() || o.is_top()) {
      CRAB_LOG("separate-domain",
	       crab::outs() << "Res=" << o << "\n";);
      return o;
    } else if (o.is_bottom() || is_top()) {
      CRAB_LOG("separate-domain",
	       crab::outs() << "Res=" << *this << "\n";);
      return *this;
    } else {
      join_op op;
      patricia_tree_t res = apply_operation(op, m_tree, o.m_tree);
      auto out = separate_discrete_domain_t(std::move(res));
      CRAB_LOG("separate-domain",
	       crab::outs() << "Res=" << out << "\n";);
      return out;
    }
  }

  separate_discrete_domain_t
  operator&(const separate_discrete_domain_t &o) const {
    CRAB_LOG("separate-domain",
	     crab::outs() << "Meet " << *this << " and " << o << "=";);

    if (is_top() || o.is_bottom()) {
      CRAB_LOG("separate-domain",
	       crab::outs() << "Res=" << o << "\n";);

      return o;
    } else if (o.is_top() || is_bottom()) {
      CRAB_LOG("separate-domain",
	       crab::outs() << "Res=" << *this << "\n";);

      return *this;
    } else {
      meet_op op;
      patricia_tree_t res = apply_operation(op, m_tree, o.m_tree);
      auto out = separate_discrete_domain_t(std::move(res));
      CRAB_LOG("separate-domain",
	       crab::outs() << "Res=" << out << "\n";);
      return out;
    }
  }

  void set(const Key &k, value_type v) {
    // Note that we can store a key-value pair where the value is
    // bottom because bottom means empty set.
    if (!is_bottom()) {
      if (v.is_top()) {
	m_tree.remove(k);
      } else {
	m_tree.insert(k, v);
      }
    }
  }

  separate_discrete_domain_t &operator-=(const Key &k) {
    if (!is_bottom()) {
      m_tree.remove(k);
    }
    return *this;
  }

  value_type operator[](const Key &k) {
    if (is_bottom()) {
      CRAB_ERROR("separate_discrete_domain::operator[] is undefined on bottom");
    } else if (is_top()) {
      return value_type::top();
    } else {
      if (boost::optional<value_type> v = m_tree.lookup(k)) {
        return *v;
      } else {
        return value_type::top();
      }
    }
  }

  void project(const std::vector<Key> &keys) {
    if (is_bottom() || is_top()) {
      return;
    }
    const int factor = 60;   // between 0 and 100
    const int small_env = 5; // size of a small environment
    int num_total_keys = (int)m_tree.size();
    int num_keys = (int)keys.size();
    if (num_total_keys <= small_env ||
        num_keys < num_total_keys * factor / 100) {
      // project on less than factor% of keys: we copy
      separate_discrete_domain_t env;
      for (auto key : keys) {
        env.set(key, operator[](key));
      }
      std::swap(*this, env);
    } else {
      // project on more or equal then factor% of keys: that
      // might be too many copies so we remove instead.
      std::vector<Key> sorted_keys(keys);
      std::sort(sorted_keys.begin(), sorted_keys.end());
      std::vector<Key> project_out_keys;
      project_out_keys.reserve(num_total_keys);
      for (auto it = m_tree.begin(); it != m_tree.end(); ++it) {
        Key key = it->first;
        if (!std::binary_search(sorted_keys.begin(), sorted_keys.end(), key)) {
          project_out_keys.push_back(key);
        }
      }
      for (auto const &key : project_out_keys) {
        this->operator-=(key);
      }
    }
  }

  // Assume that from does not have duplicates.
  void rename(const std::vector<Key> &from, const std::vector<Key> &to) {
    if (is_top() || is_bottom()) {
      // nothing to rename
      return;
    }
    if (from.size() != to.size()) {
      CRAB_ERROR(
          "separate_discrete_domain::rename with input vectors of different sizes");
    }

    if (::crab::CrabSanityCheckFlag) {
      std::set<Key> s1, s2;
      s1.insert(from.begin(), from.end());
      if (s1.size() != from.size()) {
        CRAB_ERROR("separate_discrete_domain::rename expects no duplicates");
      }
      s2.insert(to.begin(), to.end());
      if (s2.size() != to.size()) {
        CRAB_ERROR("separate_discrete_domain::rename expects no duplicates");
      }
    }

    for (unsigned i = 0, sz = from.size(); i < sz; ++i) {
      Key k = from[i];
      Key new_k = to[i];
      if (k == new_k) { // nothing to rename
        continue;
      }
      if (::crab::CrabSanityCheckFlag) {
        if (m_tree.lookup(new_k)) {
          CRAB_ERROR("separate_discrete_domain::rename assumes that  ",
		     new_k, " does not exist in ", *this);
        }
      }
      if (boost::optional<value_type> val_opt = m_tree.lookup(k)) {
        if (!(*val_opt).is_top()) {
          m_tree.insert(new_k, *val_opt);
        }
        m_tree.remove(k);
      }
    }
  }

  void write(crab::crab_os &o) const {
    if (is_top()) {
      o << "{}";
    } else if (is_bottom()) {
      o << "_|_";
    } else {
      o << "{";
      for (typename patricia_tree_t::iterator it = m_tree.begin();
           it != m_tree.end();) {
        Key k = it->first;
        k.write(o);
        o << " -> ";
        value_type v = it->second;
        v.write(o);
        ++it;
        if (it != m_tree.end()) {
          o << "; ";
        }
      }
      o << "}";
    }
  }
}; // class separate_discrete_domain

template <typename Key, typename Element>
inline crab_os &operator<<(crab_os &o,
                           const separate_discrete_domain<Key, Element> &env) {
  env.write(o);
  return o;
}

} //end namespace domains
} //end namespace crab


#if 0
#include "PMVSPatch.h"
#include <boost/filesystem.hpp>
#include <fstream>
#include <iostream>
#include "Utility.h"

namespace bfs = boost::filesystem;

Patch::Patch()
:m_visCnt1(0)
,m_visCnt2(0)
,m_normAvaiable(true)
{
}

mat_3f ViewInfo::visNorm(){
	//
	int ih = m_viewImg.size().height;
	int iw = m_viewImg.size().width;
	mat_3f vis(m_viewImg.size());
	vis = cv::Vec3f(0,0,0);
	for (int i = 0; i < ih; i++)
	{
		for (int j=0; j < iw; j++)
		{
			if (cv::norm(m_pixNorm(i,j)) < 1e-8)
			{
				vis(i,j) = cv::Vec3f(0,0,0);
				continue;
			}
			cv::Vec3f nv = m_pixNorm(i,j);
			nv = nv + cv::Vec3f(1,1,1);
			nv[0] /= 2;
			nv[1] /= 2;
			nv[2] /= 2;
			vis(i,j)[0] = nv[2];
			vis(i,j)[1] = nv[1];
			vis(i,j)[2] = nv[0];
		}
	}
	return vis;
}

void ViewInfo::save(){
	//save the pixel norm visualization
	bfs::path p1(m_viewImgFile);
	bfs::path p2 = p1.branch_path();
	std::string imgName = bfs::basename(p1);
	std::string imgExt = bfs::extension(p1);
	std::string visName = imgName + "_vis";
	std::string visPath = p2.string() + "/" + visName + ".png";
	mat_3f visImg = visNorm();
	//cv::imshow("vis",visImg);
	//cv::waitKey(0);
	visImg *= 255.0;
	cv::imwrite(visPath,visImg);
}

Patch::~Patch(){

}

ViewInfo::ViewInfo()
:m_viewIdx(-1)
,m_centerPos(cv::Vec3f(-1,-1,-1))
,m_ccEstiamted(false)
{

}
void ViewInfo::createStruct(mat_3f img, int viewIdx,std::string viewImgFile){
	m_viewIdx = viewIdx;
	m_viewImgFile = viewImgFile;
	int h = img.size().height;
	int w = img.size().width;
	m_viewImg = img.clone();
	m_pixNorm = mat_3f(h,w);
	m_normMask = mat_u(h,w);
	m_roiMask = mat_u(h,w);
	m_roiMask = 255;
	m_prjMat = mat_f(3,4);
	//////////////////////////////////////////////////////////////////////////
	m_pixNorm = cv::Vec3f(0,0,0);
	m_normMask = 0;
	m_prjMat = 0;
}

void ViewInfo::_estiamteCameraCenter(){
	int ih = m_viewImg.size().height;
	int iw = m_viewImg.size().width;

	//infer the camera center from perspective matrix
	if (!m_ccEstiamted)
	{

		cv::Vec2f v1(0,0),v2(iw - 1, 0),v3( 0 , ih -1 ), v4(iw - 1, ih - 1);
		std::vector<cv::Vec2f> ptVec;
		ptVec.push_back(v1);
		ptVec.push_back(v2);
		ptVec.push_back(v3);
		ptVec.push_back(v4);
		//
		m_centerPos = convergePoint(m_prjMat,ptVec);
		m_ccEstiamted = true;
	}

}

bool theSameSide(cv::Vec3f p1,cv::Vec3f p2, cv::Vec3f v1, cv::Vec3f v2){
	//
	cv::Vec3f v3 = v2 - v1;
	cv::Vec3f v4 = p1 - v1;
	cv::Vec3f v5 = p2 - v1;
	//
	cv::Vec3f prod1 = v3.cross(v4);
	cv::Vec3f prod2 = v3.cross(v5);
	float dProd = prod1.ddot(prod2);
	if (dProd >= 0)
	{
		return true;
	}
	else{
		return false;
	}
}

bool insideTriangle(cv::Vec2f p11, cv::Vec2f p1, cv::Vec2f p2, cv::Vec2f p3){
	cv::Vec3f p = cv::Vec3f(p11[0],p11[1],0);
	cv::Vec3f a = cv::Vec3f(p1[0],p1[1],0);
	cv::Vec3f b = cv::Vec3f(p2[0],p2[1],0);
	cv::Vec3f c = cv::Vec3f(p3[0],p3[1],0);
	if (theSameSide(p,a,b,c) && theSameSide(p,b,a,c) && theSameSide(p,c,a,b))
	{
		return true;
	}
	else{
		return false;
	}
}

float triangleArea(float a, float b, float c){
	//Heron's formula
	float s = (a + b + c) * 0.5;
	return sqrt(s * (s - a) * (s - b) * (s - c));
}

void ViewInfo::interplation(){
	//do interplation
//	vertexSet vSet;
//	int iw = m_viewImg.size().width;
//	int ih = m_viewImg.size().height;
//	for (int i = 0; i < ih; i++)
//	{
//		for (int j = 0; j < iw; j++)
//		{
//			if (m_normMask(i,j))
//			{
//				//
//				vSet.insert(vertex(j,i));
//			}
//		}
//	}
//	//now compute the delaunay
//	Delaunay del;
//	triangleSet trgs;
//	del.Triangulate(vSet,trgs);
//	std::cout << "3d normal interpolation by Delaunay triangulation" << std::endl;
//	std::cout << "# of triangles:" << trgs.size() << std::endl;
//	//
//	ctIterator iter;
//	cv::Vec2f vArry[3];
//	cv::Vec3f nrArry[3];
//	for (iter = trgs.begin(); iter != trgs.end(); iter++)
//	{
//		//compute the triangle side length
//		float minX = iw, maxX = -1, minY = ih, maxY = -1;
//
//		for (int j = 0; j < 3; j++)
//		{
//			const vertex* tmpV = iter->GetVertex(j);
//			int x = tmpV->GetX();
//			int y = tmpV->GetY();
//			vArry[j] = cv::Vec2f(x,y);
//			nrArry[j] = m_pixNorm(y,x); //get the pixel normal
//			if (x < minX)
//			{
//				minX = x;
//			}
//			if (x > maxX)
//			{
//				maxX = x;
//			}
//			if (y < minY)
//			{
//				minY = y;
//			}
//			if (y > maxY)
//			{
//				maxY = y;
//			}
//		}
//		//
//		float a = cv::norm(vArry[0] - vArry[1]);
//		float b = cv::norm(vArry[1] - vArry[2]);
//		float c = cv::norm(vArry[2] - vArry[0]);
//
//		//discard triangle with large side length
//		if (a > 11 || b > 11 || c > 11)
//		{
//			continue;
//		}
//		//discard triangle with different normals
//		float pd1 = nrArry[0].ddot(nrArry[1]);
//		float pd2 = nrArry[0].ddot(nrArry[2]);
//
//		if (pd1 < 0.90 || pd2 < 0.90)
//		{
//			continue;
//		}
//
//
//		//for each triangle, fill the pixel inside
//		for (int y = minY; y <= maxY; y++)
//		{
//			for (int x = minX; x <= maxX; x++)
//			{
//				cv::Vec2f p(x,y);
//				if (insideTriangle(p,vArry[0],vArry[1],vArry[2]))
//				{
//					//
//					float dist1 = cv::norm(p - vArry[0]);
//					float dist2 = cv::norm(p - vArry[1]);
//					float dist3 = cv::norm(p - vArry[2]);
//					float ws = 1 / (dist1 + dist2 + dist3);
//					float w1 = dist1 * ws;
//					float w2 = dist2 * ws;
//					float w3 = dist3 * ws;
//					cv::Vec3f iv;
//					iv[0] = nrArry[0][0] * w1 + nrArry[1][0] * w2 + nrArry[2][0] * w3;
//					iv[1] = nrArry[0][1] * w1 + nrArry[1][1] * w2 + nrArry[2][1] * w3;
//					iv[2] = nrArry[0][2] * w1 + nrArry[1][2] * w2 + nrArry[2][2] * w3;
//					//std::cout << "interp..." << std::endl;
//					m_pixNorm(y,x) = iv;
//					//m_normMask(y,x) = 255;
//				}
//			}
//		}
//	}
//	//////////////////////////////////////////////////////////////////////////
//	std::cout << "done..." << std::endl;
}

void ViewInfo::applyMask(){
	//
	int h = m_viewImg.size().height;
	int w = m_viewImg.size().width;

	int x1 = w, y1 = h ,x2 = -1, y2 = -1;

	for (int i = 0; i < h; i++)
	{
		for (int j = 0; j < w; j++)
		{
			if (m_roiMask(i,j))
			{
				if (i < y1)
				{
					y1 = i;
				}
				if (i > y2)
				{
					y2 = i;
				}
				if (j < x1)
				{
					x1 = j;
				}
				if (j > x2 )
				{
					x2 = j;
				}
			}
		}
	}
	int w1 = x2 - x1 + 1;
	int h1 = y2 - y1 + 1;
	//apply the roi to the norm image and input image
	mat_3f viewImg(h1,w1);
	mat_3f normImg(h1,w1);
	mat_u nmImg(h1,w1);
	for (int i = y1; i <= y2; i++)
	{
		for (int j = x1; j <= x2; j++)
		{
			viewImg(i - y1,j - x1) = m_viewImg(i,j);
			normImg(i - y1,j - x1) = m_pixNorm(i,j);
			nmImg(i - y1, j - x1) = m_normMask(i,j);
		}
	}
	m_viewImg = viewImg;
	m_normMask = nmImg;
	m_pixNorm = normImg;
	m_roiMask = mat_u(h1,w1);
	m_roiMask = 255;
	//cv::imshow("roiImg",m_viewImg);
	//cv::imshow("normVis",visNorm());
	//cv::waitKey(0);
}

void ViewInfo::projectPatch(std::vector<Patch>& patchList){
	//project each patch's normal to the image plane

	_estiamteCameraCenter();
	int ih = m_viewImg.size().height;
	int iw = m_viewImg.size().width;

	mat_f homoVec(4,1);
	homoVec(3,0) = 1.0;

	std::cout << "# of patches to project:" << patchList.size() << std::endl;
	for (int i = 0; i < patchList.size(); i++)
	{
		Patch& pch = patchList[i];
		if (pch.m_normAvaiable&& pch.m_visFlag[m_viewIdx])
		{
			//project current patch
			homoVec(0,0) = pch.m_pt[0];
			homoVec(1,0) = pch.m_pt[1];
			homoVec(2,0) = pch.m_pt[2];
			mat_f imgPos = m_prjMat * homoVec;
			imgPos(2,0) = imgPos(2,0) > 1e-8 ? 1 / imgPos(2,0) : 0;
			imgPos(0,0) *= imgPos(2,0);
			imgPos(1,0) *= imgPos(2,0);
			int px = floor(imgPos(0,0));
			int py = floor(imgPos(1,0));

			if (py < 0 || py > ih || px < 0 || px > iw)
			{
				continue;
			}

			//correct the normal direction
			cv::Vec3f& ptNorm = pch.m_norm;
			cv::Vec3f viewVec = m_centerPos - pch.m_pt;
			if (ptNorm.ddot(viewVec) < 0)
			{
				ptNorm = cv::Vec3f(0,0,0) - ptNorm;
			}
			m_pixNorm(py,px) += ptNorm;
			m_normMask(py,px) = 255;
			//for (int k1 = -1; k1 <= 1; k1++)
			//{
			//	for (int k2 = -1; k2 <= 1; k2++)
			//	{
			//		int x1 = px + k2;
			//		int y1 = py + k1;
			//		if (x1 < 0 || x1 >= iw || y1 < 0 || y1 >= ih)
			//		{
			//			continue;
			//		}
			//		//now correct the normal direction
			//		m_pixNorm(y1,x1) += pch.m_norm;
			//		m_normMask(y1,x1) = 255;
			//	}
			//}
		}
	}
	//interplation();
	for (int i = 0; i < ih ;i++)
	{
		for (int j = 0; j < iw; j++)
		{
			if (m_normMask(i,j))
			{
				cv::Vec3f& pn = m_pixNorm(i,j);
				float nr = cv::norm(pn);
				nr = nr > 1e-8 ? 1/nr: nr;
				pn[0] *= nr;
				pn[1] *= nr;
				pn[2] *= nr;
			}
		}
	}

}

ViewInfo::~ViewInfo(){

}

PMVS2::PMVS2(std::string pmvsRootPath)
:m_pmvsRootPath(pmvsRootPath){
	m_txtPath = m_pmvsRootPath + "txt/";
	m_visPath = m_pmvsRootPath + "visualize/";
	m_modelPath = m_pmvsRootPath + "models/";
	//
	_init();
}

void PMVS2::_getViewCnt(){
	std::vector<std::string> m_viewImgFile;
	Utility::findFilexByExt(m_visPath,".jpg",m_viewImgFile);
	m_viewCnt = m_viewImgFile.size();
}

void PMVS2::_init(){
	_getViewCnt();
	std::cout << "# of views:" << m_viewCnt << std::endl;
	//reserve space
	std::cout << "init view information" << std::endl;
	for (int i = 0; i < m_viewCnt; i++)
	{
		m_viewInfo.push_back(ViewInfo());
	}

	//load the matrix
	std::cout << "load camera parameters" << std::endl;
	_loadCamMatrix();
	//load ply and .patch files
	std::cout << "load 3d information" << std::endl;
	_loadModel();
	//
}

void PMVS2::_loadCamMatrix(){
	char buffer[512];
	std::fstream camFS;
	std::string line;
	mat_f camMat(3,4);
	for (int i = 0; i < m_viewCnt; i++)
	{
		sprintf(buffer,"%s%04d.txt",m_txtPath.c_str(),i);
		//std::cout << buffer << std::endl;
		camFS.open(buffer,std::ios_base::in);
		assert(camFS.good());
		camFS >> line;
		for (int j = 0; j < 3; j++)
		{
			for (int k = 0; k < 4; k++)
			{
				camFS >> camMat(j,k);
#ifdef __DUMP_FILE__
				std::cout << camMat(j,k) << " ";
#endif
			}
#ifdef __DUMP_FILE__
			std::cout << std::endl;
#endif
		}
		m_camParaList.push_back(camMat.clone());
		camFS.close();
	}
	std::cout << "# of matrix loaded:" << m_viewCnt << std::endl;
	//////////////////////////////////////////////////////////////////////////
}

void PMVS2::loadViewInfo(int viewIdx ){

	if (viewIdx == -1)
	{
		for (int i = 0; i < m_viewCnt; i++)
		{
			loadViewInfo(i);
		}
	}
	else{
		char buffer[512];
		sprintf(buffer,"%s%04d.jpg",m_visPath.c_str(),viewIdx);
		//std::cout << buffer << std::endl;
		mat_3f img = cv::imread(buffer) / 255.0;
#ifdef __DUMP_FILE__
		cv::imshow("viewImg",img);
		cv::waitKey(0);
#endif
		int ih = img.size().height;
		int iw = img.size().width;
		ViewInfo vi;
		vi.createStruct(img,viewIdx,buffer);
		vi.m_prjMat = m_camParaList[viewIdx];
		m_viewInfo[viewIdx] = vi;
		//project the patches to image plane
		m_viewInfo[viewIdx].projectPatch(m_patchList);
		///read the mask file
		sprintf(buffer,"%s%04d_msk.png",m_visPath.c_str(),viewIdx);
		if (bfs::exists(buffer))
		{
			mat_u mskImg = cv::imread(buffer,CV_LOAD_IMAGE_GRAYSCALE);
			//cv::imshow("roimsk",mskImg);
			//cv::waitKey(0);
			m_viewInfo[viewIdx].setMask(mskImg);
			m_viewInfo[viewIdx].applyMask();
		}

	}
}

//
//void PMVS2::doProjection(){
//	for (int i = 0; i < m_viewCnt; i++)
//	{
//		std::cout << "project patches to view:" << i << std::endl;
//		ViewInfo& vi = m_viewInfo[i];
//		vi.projectPatch(m_patchList);
//		vi.save();
//		//std::cout << "show norm" << std::endl;
//		//cv::imshow("norm",vi.visNorm());
//		//cv::waitKey(0);
//	}
//}

void PMVS2::_saveEstimatedNormalAsPly(){
	//
	std::string folder = m_modelPath + "/est";
	if (!bfs::exists(folder))
	{
		bfs::create_directories(folder);
	}
	std::string plyFile = folder + "/estNorm.ply";
	std::fstream plyFS;
	plyFS.open(plyFile.c_str(),std::ios::out);
	plyFS << "ply\n";
	plyFS << "format ascii 1.0\n";
	plyFS << "element vertex " << m_patchList.size() << "\n";
	plyFS << "property float x\n";
	plyFS << "property float y\n";
	plyFS << "property float z\n";
	plyFS << "property float nx\n";
	plyFS << "property float ny\n";
	plyFS << "property float nz\n";
	plyFS << "property uchar red\n";
	plyFS << "property uchar green\n";
	plyFS << "property uchar blue\n";
	plyFS << "property uchar alpha\n";

	plyFS << "element face 0\n";
	plyFS << "property list uchar int vertex_indices\n";
	plyFS << "end_header\n";

	for (int i = 0; i < m_patchList.size(); i++)
	{
		Patch& pch = m_patchList[i];
		plyFS <<
			pch.m_pt[0] << " " << pch.m_pt[1] << " " << pch.m_pt[2] << " "
			<< pch.m_norm[0] << " " << pch.m_norm[1] << " " << pch.m_norm[2] << " "
			<< (int)pch.m_r << " " << (int)pch.m_g << " " << (int)pch.m_b << " " << 255 << "\n";
	}
	plyFS << std::endl;
	plyFS.close();
}

void PMVS2::_loadModel(){
	std::vector<std::string> modelFiles;
	Utility::findFilexByExt(m_modelPath,".patch",modelFiles);
	if (modelFiles.empty())
	{
		std::cout << "patch file not found" << std::endl;
	}
	else{
		m_patchFile = modelFiles[0];
	}
	modelFiles.clear();
	Utility::findFilexByExt(m_modelPath,".ply",modelFiles);
	if (modelFiles.empty())
	{
		std::cout << "patch file not found" << std::endl;
	}
	else{
		m_plyFile = modelFiles[0];
	}

	modelFiles.clear();
	Utility::findFilexByExt(m_modelPath,".pset",modelFiles);
	if (modelFiles.empty())
	{
		std::cout << "patch file not found" << std::endl;
	}
	else{
		m_ptFile = modelFiles[0];
	}

	//
	_loadPatch();
	//
	//_loadPly();
	////
	_estimateNormal();
	_saveEstimatedNormalAsPly();
	//////////////////////////////////////////////////////////////////////////
}

void PMVS2::_estimateNormal(){
	//
	NormalEstimator ne;
	std::vector<cv::Vec3f> ptList;
	for (int i = 0; i < m_patchList.size(); i++)
	{
		Patch& pch = m_patchList[i];
		ptList.push_back(pch.m_pt);
	}
	ne.specifyPointSet(ptList);
	ne.fitNormal();
	for (int i = 0; i < m_patchList.size(); i++)
	{
		m_patchList[i].m_normAvaiable = ne.m_ptNormFlag[i];
		m_patchList[i].m_norm = ne.m_ptNorm[i];
	}

	//
	//now update the normal
	//for (int i = 0; i < m_patchList.size(); i++)
	//{
	//	cv::Vec3f v2 = ne.m_ptNorm[i];
	//	if (ne.m_ptNormFlag[i])
	//	{
	//		//correct the direction
	//		cv::Vec3f v1 = m_patchList[i].m_norm;
	//		float pd1 = v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
	//		//float pd2 = v1.ddot(cv::Vec3f(0,0,0) - v2);
	//		if (pd1 < 0)
	//		{
	//			v2 = cv::Vec3f(0,0,0) - v2;
	//		}
	//	}
	//	m_patchList[i].m_norm = v2;
	//}
	//
}

mat_f ViewInfo::eqMatFromImgPt(mat_f& prjMat, cv::Vec2f& imgPt){
	//
	float u = imgPt[0];
	float v = imgPt[1];
	mat_f lMat(2, 4);

	lMat(0, 0) = prjMat(2, 0)*u - prjMat(0, 0);
	lMat(0, 1) = prjMat(2, 1)*u - prjMat(0, 1);
	lMat(0, 2) = prjMat(2, 2)*u - prjMat(0, 2);

	lMat(1, 0) = prjMat(2, 0)*v - prjMat(1, 0);
	lMat(1, 1) = prjMat(2, 1)*v - prjMat(1, 1);
	lMat(1, 2) = prjMat(2, 2)*v - prjMat(1, 2);

	lMat(0, 3) = -prjMat(0, 3) + prjMat(2, 3)*u;
	lMat(1, 3) = -prjMat(1, 3) + prjMat(2, 3)*v;

	return lMat;
}
cv::Vec3f ViewInfo::convergePoint(mat_f& prjMat, std::vector<cv::Vec2f>& ptSeq){
	//
	int ptCnt = ptSeq.size();
	mat_f lMat(2*ptCnt, 3), rMat(2*ptCnt, 1);
	for (int i=0; i<ptCnt; i++) {
		mat_f eqM = eqMatFromImgPt(prjMat, ptSeq[i]);
		lMat(i*2, 0) = eqM(0, 0);
		lMat(i*2, 1) = eqM(0, 1);
		lMat(i*2, 2) = eqM(0, 2);
		lMat(i*2+1, 0) = eqM(1, 0);
		lMat(i*2+1, 1) = eqM(1, 1);
		lMat(i*2+1, 2) = eqM(1, 2);
		rMat(i*2, 0) = -eqM(0, 3);
		rMat(i*2+1, 0) = -eqM(1, 3);
	}
	mat_f x(3, 1);
	cv::solve(lMat, rMat, x, cv::DECOMP_SVD);
	return cv::Vec3f(x(0, 0), x(1, 0), x(2, 0));
}

ViewInfo& PMVS2::getViewInfo(int viewIdx){
	if (viewIdx < 0 || viewIdx >= m_viewCnt)
	{
		viewIdx = 0;
	}
	return m_viewInfo[viewIdx];
}

void PMVS2::deleteViewInfo(int idx ){
	if (idx == -1)
	{
		for (int i = 0; i < m_viewCnt; i++)
		{
			deleteViewInfo(i);
		}
	}
	else{
		m_viewInfo[idx] = ViewInfo();
	}

}
void PMVS2::_loadPly(){
	std::fstream plyFS;
	plyFS.open(m_plyFile.c_str(),std::ios_base::in);
	assert(plyFS.good());
	std::string line;
	char buffer[512];
	plyFS.getline(buffer,512);
	//std::cout << buffer;
	plyFS.getline(buffer,512);
	//std::cout << buffer;
	plyFS.getline(buffer,512);
	//std::cout << buffer;

	int vertNum;
	plyFS >> line;
	plyFS >> line;
	plyFS >> vertNum;
	std::cout << "# of vertex:" << vertNum << std::endl;
	assert(vertNum == m_patchList.size());

	for (int i = 0; i < 15; i++)
	{
		plyFS.getline(buffer,512);
		//std::cout << buffer << std::endl;
	}

	float x,y,z,nx,ny,nz;
	float r,g,b, alpha;

	for (int i = 0; i < vertNum; i++)
	{
		plyFS >> x >> y >> z >> nx >> ny >> nz >> r >> g >> b >> alpha;
		//std::cout << x << " " << y << " " << z << " " << nx << " " << ny << " " << nz << " " << r << " " << g << " " << b << " " << alpha << std::endl;
		//getchar();
		Patch& pch = m_patchList[i];
		pch.m_norm = cv::Vec3f(nx,ny,nz);
		pch.m_r = r;
		pch.m_g = g;
		pch.m_b = b;
	}
	plyFS.close();
	//
}

void PMVS2::_loadPatch(){
	assert(bfs::exists(m_patchFile));
	std::fstream patchFS;
	patchFS.open(m_patchFile.c_str(),std::ios_base::in);
	assert(patchFS.good());
	std::string line;
	int ptCnt;
	patchFS >> line; //PATCHES
	patchFS >> ptCnt; //# of patches
	float x, y ,z , nx , ny , nz;
	float dummy;
	int visCnt1,visCnt2, visIdx;

	std::vector<bool> visFlag(m_viewCnt,false);

	for (int i = 0; i < ptCnt; i++)
	{
		Patch p;
		p.m_visFlag = visFlag;
		patchFS >> line; //PATCHES
		//std::cout << line << std::endl;
		patchFS >> p.m_pt[0] >> p.m_pt[1] >> p.m_pt[2] >> dummy;

		patchFS >> p.m_norm[0] >> p.m_norm[1] >> p.m_norm[2] >> dummy;

		patchFS >> p.m_photoConsistency >> p.m_debug1 >> p.m_debug2;
		patchFS >> p.m_visCnt1;
		if (p.m_photoConsistency < 0)
		{
			continue;
		}
		for (int j = 0; j < p.m_visCnt1; j++)
		{
			patchFS >> visIdx;
			p.m_visIndex1.push_back(visIdx);
			p.m_visIndex.insert(visIdx);
			p.m_visFlag[visIdx] = true;
		}
		patchFS >> p.m_visCnt2;

		for (int j = 0; j < p.m_visCnt2; j++)
		{
			patchFS >> visIdx;
			p.m_visIndex2.push_back(visIdx);
			p.m_visIndex.insert(visIdx);
			//p.m_visFlag[visIdx] = true;
		}
		//patchFS >> line; //empty line

#ifdef __DUMP_FILE__
		std::cout << p.m_pt[0] << " " << p.m_pt[1] << " " << p.m_pt[2] << std::endl;
		std::cout << p.m_norm[0] << " " << p.m_norm[1] << " " << p.m_norm[2] << std::endl;
		std::cout << p.m_photoConsistency << " " << p.m_debug1 << " " << p.m_debug2 << std::endl;
		std::cout << p.m_visCnt1 << " " << p.m_visCnt2 << std::endl;
		getchar();
#endif
		m_patchList.push_back(p);
	}
	//////////////////////////////////////////////////////////////////////////
	patchFS.close();
	std::cout <<"# of patches loaded:" << m_patchList.size() << std::endl;
}

PMVS2::~PMVS2(){

}
#endif


#include <cmath>
#include <iostream>
#include <fstream>
#include <string>
#include <sstream>
#include <numeric>
#include <Eigen/Dense>
#include <boost/unordered_map.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_01.hpp>
#include <padenti/cv_image_loader.hpp>
#include <padenti/uniform_image_sampler.hpp>
#include <padenti/mh_rtree.hpp>
#include <padenti/cl_regressor.hpp>

#include "rchord_meanshift.hpp"

#define RDIM (4)
#define N_SAMPLES (2048)

// Default parameters
#define DEFAULT_MS_SIGMA       (0.7321)
#define DEFAULT_MS_STEPSIZE    (0.2)
#define DEFAULT_MS_STEPTHR     (0.025)
#define DEFAULT_MS_SAMECTHR    (0.15)
#define DEFAULT_MS_NITERTHR    (10)
#define DEFAULT_RESERVOIR_SIZE (200)

// MeanShift number of hypothesis
#define MS_NHYPH    (2)

#define USE_CPU (false)

typedef CVImageLoader<unsigned short, 1> DepthmapLoaderT;
typedef UniformImageSampler<unsigned short, 1> SamplerT;
typedef RTree<short int, 2, RDIM> RTreeT;
typedef RTreeNode<short int, 2, RDIM> RTreeNodeT;
typedef MHRTree<short int, 2, RDIM, MS_NHYPH> MHRTreeT;
typedef CLRegressor<unsigned short, 1, short, 2, RDIM> RegressorT;
typedef Image<unsigned short, 1> DepthmapT;
typedef Image<unsigned char, 1> MaskT;
typedef Image<int, 1> PredictionT;

int main(int argc, const char *argv[])
{
  if (argc!=4 && argc!=10)
  {
    std::cout << "Usage: "
	      << argv[0] << " TREE_FILE.xml DEPTH_LIST_FILE.txt EULER_ANGLES.txt [MS_BANDWIDTH MS_STEPSIZE MS_STEPTHR MS_GROUPINGTHR MS_NITERTHR MS_RESERVOIRSIZE]"
	      << std::endl;
    return 1;
  }

  boost::random::mt19937 gen;
  boost::random::uniform_01<> dist;

  // Setup the tree and classifier
  RTreeT tree;
  RegressorT regressor(".", USE_CPU);
  tree.load(argv[1]);
  regressor << tree;

  // Read the meanshift parameters from commandline
  float MS_SIGMA, MS_STEPSIZE, MS_STEPTHR, MS_SAMECTHR, MS_NITERTHR, RESERVOIR_SIZE;
  if (argc==4)
  {
    MS_SIGMA = DEFAULT_MS_SIGMA;
    MS_STEPSIZE = DEFAULT_MS_STEPSIZE;
    MS_STEPTHR = DEFAULT_MS_STEPTHR;
    MS_SAMECTHR = DEFAULT_MS_SAMECTHR;
    MS_NITERTHR = DEFAULT_MS_NITERTHR;
    RESERVOIR_SIZE = DEFAULT_RESERVOIR_SIZE;
  }
  else
  {
    MS_SIGMA = atof(argv[4]);
    MS_STEPSIZE = atof(argv[5]);
    MS_STEPTHR = atof(argv[6]);
    MS_SAMECTHR = atof(argv[7]);
    MS_NITERTHR = atof(argv[8]);
    RESERVOIR_SIZE = atoi(argv[9]);
  }

  // Scan the tree's nodes to find leaves
  boost::unordered_map<int, int> leafIdxMap;
  std::vector<int> leafIdx;
  unsigned int nNodes = (2<<(tree.getDepth()-1))-1, nLeaves=0;
  for (int i=0; i<nNodes; i++)
  {
    const RTreeNodeT &currNode = tree.getNode(i);
    if (*currNode.m_leftChild!=-1) continue;

    leafIdx.push_back(i);
    leafIdxMap[i] = nLeaves++;
  }

  // Build the vectors which will host per-leaf rotations
  std::vector<std::vector<Eigen::Vector3f> > perLeafSamples(nLeaves);
  std::vector<int> perLeafNSamples(nLeaves);
  std::fill(perLeafNSamples.begin(), perLeafNSamples.end(), 0);

  // Load the training set
  DepthmapLoaderT depthmapLoader;
  SamplerT sampler(N_SAMPLES, 0);

  std::vector<Eigen::Vector3f> rotations;
  std::ifstream depthList(argv[2]);
  std::ifstream eulerList(argv[3]);
  std::string depthStr, eulerStr;
  while (getline(depthList, depthStr) && getline(eulerList, eulerStr))
  {
    // Read the Euler triplet for current depth (in XYZ) and keep it in ZYX
    std::stringstream anglesStr(eulerStr);
    Eigen::Vector3f angles;

    anglesStr >> angles.z();
    anglesStr >> angles.y();
    anglesStr >> angles.x();
    angles *= M_PI/180.;

    // Read the depthmap and sample it
    DepthmapT depthmap = depthmapLoader.load(depthStr);
    unsigned int samples[N_SAMPLES], nSamples;

    // Trick: create the labels used for sampling as a simple mask of non-zero depthmap pixels
    MaskT mask(depthmap.getWidth(), depthmap.getHeight());
    for (int i=0; i<depthmap.getWidth()*depthmap.getHeight(); i++)
    {
      mask.getData()[i] = static_cast<unsigned char>(depthmap.getData()[i]>0);
    }

    nSamples = sampler.sample(depthmap.getData(), mask.getData(),
			      depthmap.getWidth(), depthmap.getHeight(), samples);

    // Perform leaf prediction and update the list of per leaf rotation using
    // reservoir sampling
    PredictionT prediction(depthmap.getWidth(), depthmap.getHeight());
    regressor.predict(0, depthmap, prediction, mask);
    for (int i=0; i<nSamples; i++)
    {
      int nodeId = prediction.getData()[samples[i]];
      std::vector<Eigen::Vector3f> &nodeRot = perLeafSamples.at(leafIdxMap.at(nodeId));

      int nodeNSamples = perLeafNSamples.at(leafIdxMap.at(nodeId));
      if (nodeNSamples < RESERVOIR_SIZE)
      {
	nodeRot.push_back(angles);
      }
      else
      {
	unsigned int j = roundf(dist(gen)*nodeNSamples);
	if (j<RESERVOIR_SIZE)
	{
	  nodeRot.at(j) = angles;
	}
      }
      perLeafNSamples.at(leafIdxMap.at(nodeId))++;
    }
  }

  std::cout << "Leaves' samples list built, start mean shift regression" << std::endl;

  // Init the multihyphotesis tree
  // - create an empty tree
  MHRTreeT mhRTree(tree.getID(), tree.getDepth());
  // - copy the content from the classification tree
  std::copy(tree.getLeftChildren(), tree.getLeftChildren()+nNodes, mhRTree.getLeftChildren());
  std::copy(tree.getFeatures(), tree.getFeatures()+nNodes*2, mhRTree.getFeatures());
  std::copy(tree.getThresholds(), tree.getThresholds()+nNodes, mhRTree.getThresholds());
  std::copy(tree.getValues(), tree.getValues()+nNodes*RDIM, mhRTree.getValues());

  // Perform mean shift on leaves' rotations
  std::ofstream leavesStats("rleavesStats.txt");
  for (int i=0; i<nLeaves; i++)
  {
    const std::vector<Eigen::Vector3f> &votes = perLeafSamples.at(i);
    std::vector<float> votesW(votes.size()); std::fill(votesW.begin(), votesW.end(), 1.f);
    std::vector<int> votesID(perLeafSamples.at(i).size());
    std::vector<Eigen::Vector3f> modes(MS_NHYPH);

    int nodeID = leafIdx.at(i);
    std::cout << "Node " << nodeID << ":";

    int nModes = MultiGuessRChordMeanShift(votes, votesW, votes, modes, votesID,
					   MS_SIGMA, MS_STEPSIZE, MS_STEPTHR,
					   MS_NITERTHR, MS_SAMECTHR);

    std::vector<int> modesW(nModes); std::fill(modesW.begin(), modesW.end(), 0);
    for (int j=0; j<votesID.size(); j++)
    {
      int id = votesID.at(j);
      if (id!=-1) modesW.at(id)++;
    }

    for (int j=0; j<nModes; j++)
      std::cout << " " << modes.at(j).z()
		<< " " << modes.at(j).y()
		<< " " << modes.at(j).x()
		<< " (weight: " << modesW.at(j) << ")";
    std::cout << std::endl;

    // Update the node hypothesis
    float *nodeVotesPtr = mhRTree.getVotes()+nodeID*4*MS_NHYPH;
    for (int j=0; j<MS_NHYPH; j++)
    {
      nodeVotesPtr[j*4]   = modes.at(j).z()*180.f/M_PI;
      nodeVotesPtr[j*4+1] = modes.at(j).y()*180.f/M_PI;
      nodeVotesPtr[j*4+2] = modes.at(j).x()*180.f/M_PI;
      nodeVotesPtr[j*4+3] = 0.f;
    }
    unsigned int *nodeNVotesPtr = mhRTree.getNVotes()+nodeID;
    *nodeNVotesPtr = nModes;
    float *nodeVoteWeightsPtr = mhRTree.getVoteWeights()+nodeID*MS_NHYPH;
    std::copy(modesW.begin(), modesW.begin()+nModes, nodeVoteWeightsPtr);
  }

  // Done, save the tree
  mhRTree.save("mhrtree.xml");

  return 0;
}


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */

#include "Utils/GeometricDerivatives/AdiabaticModeLocalizer.h"
#include "Utils/Bonds/BondOrderCollection.h"
#include "Utils/GeometricDerivatives/HessianUtilities.h"
#include "Utils/Geometry/AtomCollection.h"
#include "Utils/Geometry/ElementInfo.h"
#include "Utils/MolecularTrajectory.h"
#include <Eigen/Eigen>
#include <algorithm>

namespace Scine {
namespace Utils {

// Localized mode storage
void AdiabaticModesContainer::addMode(std::pair<int, int> internalCoordinate, NormalMode mode, double forceConstant) {
  // Ensure that this mode is not already there
  int storageIndex = this->getStorageIndex(internalCoordinate);
  if (storageIndex != static_cast<int>(storageOrder_.size())) {
    throw AdiabaticLocalizationExceptions::modeReadded();
  };
  // Add mode and force constant
  modesContainer_.add(std::move(mode));
  forceConstants_.insert(std::pair<std::pair<int, int>, double>(internalCoordinate, forceConstant));
  forceConstants_.insert(std::pair<std::pair<int, int>, double>(
      std::make_pair(internalCoordinate.second, internalCoordinate.first), forceConstant));
  // Store storage location
  storageOrder_.insert(
      std::pair<std::pair<int, int>, int>(std::minmax(internalCoordinate.first, internalCoordinate.second), storageIndex));
}

int AdiabaticModesContainer::getStorageIndex(std::pair<int, int> internalCoordinate) const {
  // Ensure proper sorting
  std::pair<int, int> sortedCoord = std::minmax(internalCoordinate.first, internalCoordinate.second);
  // Find value
  auto mapIterator = storageOrder_.find(sortedCoord);
  if (mapIterator != storageOrder_.end()) {
    return storageOrder_.find(sortedCoord)->second;
  }

  return storageOrder_.size();
}

const DisplacementCollection& AdiabaticModesContainer::getMode(std::pair<int, int> internalCoordinate) const {
  int storageIndex = this->getStorageIndex(internalCoordinate);
  if (storageIndex == static_cast<int>(storageOrder_.size())) {
    throw AdiabaticLocalizationExceptions::InvalidModeRequested();
  };
  return modesContainer_.getMode(storageIndex);
}

MolecularTrajectory AdiabaticModesContainer::getModeAsMolecularTrajectory(std::pair<int, int> internalCoordinate,
                                                                          const Utils::AtomCollection& atoms,
                                                                          double scalingFactor) const {
  int storageIndex = this->getStorageIndex(internalCoordinate);
  if (storageIndex == static_cast<int>(storageOrder_.size())) {
    throw AdiabaticLocalizationExceptions::InvalidModeRequested();
  };
  return modesContainer_.getModeAsMolecularTrajectory(storageIndex, atoms, scalingFactor);
}

std::map<std::pair<int, int>, double> AdiabaticModesContainer::getWaveNumbers() {
  if (wavenumbers_.size() != 2 * static_cast<unsigned>(modesContainer_.size())) {
    std::vector<double> waveVector = modesContainer_.getWaveNumbers();
    for (auto const& it : storageOrder_) {
      wavenumbers_.insert(std::pair<std::pair<int, int>, double>(std::make_pair(it.first.first, it.first.second),
                                                                 waveVector.at(it.second)));
      wavenumbers_.insert(std::pair<std::pair<int, int>, double>(std::make_pair(it.first.second, it.first.first),
                                                                 waveVector.at(it.second)));
    }
  }
  return wavenumbers_;
}

std::map<std::pair<int, int>, double> AdiabaticModesContainer::getForceConstants() const {
  return forceConstants_;
}

// Mode Localization
AdiabaticModeLocalizer::AdiabaticModeLocalizer(const Eigen::MatrixXd& hessian, AtomCollection atoms,
                                               const BondOrderCollection& bondOrders, const double bondingThreshold)
  : atoms_(std::move(atoms)), hessian_(hessian) {
  if (hessian_.rows() != 3 * atoms_.size() || hessian_.cols() != 3 * atoms_.size()) {
    throw AdiabaticLocalizationExceptions::InvalidHessianDimensions();
  }
  // Get pruned bond list
  Eigen::SparseMatrix<double> bondMatrix = bondOrders.getMatrix().pruned(bondingThreshold).triangularView<Eigen::Upper>();
  for (int j = 0; j < bondMatrix.outerSize(); ++j) {
    for (Eigen::SparseMatrix<double>::InnerIterator it(bondMatrix, j); it; ++it) {
      internalCoords_.emplace_back(it.row(), it.col());
    }
  }
}

AdiabaticModeLocalizer::AdiabaticModeLocalizer(const Eigen::MatrixXd& hessian, AtomCollection atoms,
                                               std::vector<std::pair<int, int>> internalCoordinates)
  : atoms_(std::move(atoms)), hessian_(hessian), internalCoords_(std::move(internalCoordinates)) {
  if (hessian_.rows() != 3 * atoms_.size() || hessian_.cols() != 3 * atoms_.size()) {
    throw AdiabaticLocalizationExceptions::InvalidHessianDimensions();
  }
}

void AdiabaticModeLocalizer::calculateStretchingB() {
  B_.resize(internalCoords_.size(), 3 * atoms_.size());
  B_.setZero();
  Eigen::MatrixXd positions = atoms_.getPositions();
  for (unsigned int i = 0; i != internalCoords_.size(); ++i) {
    // Bij = del q_j / del x_i
    std::pair<int, int> const coord = internalCoords_[i];
    Eigen::RowVector3d p1 = positions.row(coord.first);
    Eigen::RowVector3d p2 = positions.row(coord.second);
    double distance = (p1 - p2).norm();
    B_.row(i).segment(3 * coord.first, 3) << p1 - p2;
    B_.row(i).segment(3 * coord.second, 3) << p2 - p1;
    B_.row(i) /= distance;
  }
}

AdiabaticModesContainer AdiabaticModeLocalizer::localizeModes() {
  AdiabaticModesContainer modesContainer;
  // Following the notation of https://doi.org/10.1002/wcms.1480
  // Get Wilson B matrix
  this->calculateStretchingB();
  HessianUtilities hessianDiagonalizer(hessian_, atoms_.getElements(), atoms_.getPositions(), false);
  Eigen::MatrixXd L = hessianDiagonalizer.getBackTransformedInternalEigenvectors(); // rototranslation free eigenvectors
  Eigen::MatrixXd K = L.transpose() * hessian_ * L;                                 // force constants/eigenvalues
  Eigen::MatrixXd KInverse = K.inverse();
  Eigen::MatrixXd D = B_ * L; // internal modes
  // Get mass matrix and adiabatic masses
  ElementTypeCollection elements = atoms_.getElements();
  Eigen::DiagonalMatrix<double, Eigen::Dynamic> MInverse(3 * elements.size());
  MInverse.setIdentity();
  for (unsigned int i = 0; i != elements.size(); ++i) {
    MInverse.diagonal().segment(3 * i, 3) /= ElementInfo::mass(elements[i]);
  }
  Eigen::MatrixXd MAdiabaticInverse = B_ * MInverse * B_.transpose(); // Inverse adiabatic masses m_a, G matrix diagonal

  for (unsigned int i = 0; i != internalCoords_.size(); ++i) {
    Eigen::RowVectorXd dn = D.row(i);
    // Adiabatic force constant in a.u.
    double kAdiabatic = 1 / (dn * KInverse * dn.transpose());
    // Adiabatic wave number in a.u., stored as negative if imaginary
    double signConverter = kAdiabatic < 0 ? -1 : 1;
    double wavenumber = signConverter * sqrt(signConverter * kAdiabatic * MAdiabaticInverse.diagonal()[i]);
    // Conversion to cm^-1
    wavenumber *= Constants::invCentimeter_per_hartree * sqrt(Constants::u_per_electronRestMass);
    // Localized mode in cartesians
    Eigen::VectorXd modeAdiabatic = L * KInverse * dn.transpose() / (dn * KInverse * dn.transpose());

    // Transform to displacement collection
    DisplacementCollection dcAdiabatic(atoms_.size(), 3);
    for (int j = 0; j < atoms_.size(); ++j) {
      dcAdiabatic.row(j) = modeAdiabatic.segment(3 * j, 3);
    }
    NormalMode formattedMode(wavenumber, dcAdiabatic);
    modesContainer.addMode(internalCoords_.at(i), formattedMode, kAdiabatic);
  }
  return modesContainer;
}
} // namespace Utils
} // namespace Scine