#include <sstream>
#include <json11.hpp>
#include <boost/filesystem/fstream.hpp>
#include "picpac-cv.h"

namespace picpac {

    using namespace json11;

    float LimitSize (cv::Mat input, int min_size, int max_size, cv::Mat *output) {
        if (input.rows == 0) {
            *output = cv::Mat();
            return 0;
        }
        float scale = 1.0;
        int maxs = std::max(input.cols, input.rows);
        int mins = std::min(input.cols, input.rows);

        if ((max_size > 0) && (maxs > max_size)) {
            cv::Mat tmp;
            scale = 1.0 * maxs / max_size;
            cv::resize(input, tmp, cv::Size(input.cols * max_size / maxs, input.rows * max_size / maxs));
            input = tmp;
        }
        // large side > max
        if ((min_size > 0) && (mins < min_size)) {
            cv::Mat tmp;
            scale = 1.0 * min_size / mins;
            cv::resize(input, tmp, cv::Size(input.cols * min_size / mins, input.rows * min_size / mins));
            input = tmp;
        }
        *output = input;
        return scale;
    }

    class Box: public Shape {
    protected:
        cv::Rect_<float> rect;
    public:
        Box (Json const &geo, char const *t = "rect"): Shape(t) {
            rect.x = geo["x"].number_value();
            rect.y = geo["y"].number_value();
            rect.width = geo["width"].number_value();
            rect.height = geo["height"].number_value();
        }
        virtual void dump (Json *json) const {
            *json = Json::object {
                {"type", type()},
                {"geometry", Json::object{
                                      {"x", rect.x},
                                      {"y", rect.y},
                                      {"width", rect.width},
                                      {"height", rect.height}
                                   }}
            };
        }
        virtual std::shared_ptr<Shape> clone () const {
            return std::shared_ptr<Shape>(new Box(*this));
        }
        virtual void draw (cv::Mat *m, cv::Scalar v, int thickness) const {
            cv::Rect box;
            box.x = std::round(m->cols * rect.x);
            box.y = std::round(m->rows * rect.y);
            box.width = std::round(m->cols * rect.width);
            box.height = std::round(m->rows * rect.height);
            cv::rectangle(*m, box, v, thickness);
        }
        virtual void bbox (cv::Rect_<float> *bb) const {
            *bb = rect;
        }
        virtual void zoom (cv::Rect_<float> const &bb) {
            rect.x -= bb.x;
            rect.y -= bb.y;
            rect.x /= bb.width;
            rect.width /= bb.width;
            rect.y /= bb.height;
            rect.height /= bb.height;
        }
    };

    class Ellipse: public Box {
    public:
        Ellipse (Json const &geo): Box(geo, "ellipse") {
        }
        virtual std::shared_ptr<Shape> clone () const {
            return std::shared_ptr<Shape>(new Ellipse(*this));
        }
        virtual void draw (cv::Mat *m, cv::Scalar v, int thickness) const {
            cv::Point2f center(m->cols * (rect.x + rect.width/2),
                               m->rows * (rect.y + rect.height/2));
            cv::Size2f size(m->cols * rect.width, m->rows * rect.height);
            cv::ellipse(*m, cv::RotatedRect(center, size, 0), v, thickness);
        }
    };

    class Poly: public Shape {
        vector<cv::Point2f> points;
    public:
        Poly (Json const &geo): Shape("polygon") {
            for (auto const &p: geo["points"].array_items()) {
                points.emplace_back(p["x"].number_value(), p["y"].number_value());
            }
        }
        virtual void dump (Json *json) const {
            vector<Json> pts;
            for (auto const &p: points) {
                pts.emplace_back(Json::object{{"x", p.x}, {"y", p.y}});
            }
            *json = Json::object {
                {"type", type()},
                {"geometry", Json::object{{"points", std::move(pts)}}}
            };
        }
        virtual std::shared_ptr<Shape> clone () const {
            return std::shared_ptr<Poly>(new Poly(*this));
        }
        virtual void draw (cv::Mat *m, cv::Scalar v, int thickness) const {
            vector<cv::Point> ps(points.size());
            for (unsigned i = 0; i < ps.size(); ++i) {
                auto const &from = points[i];
                auto &to = ps[i];
                to.x = std::round(from.x * m->cols);
                to.y = std::round(from.y * m->rows);
            }
            cv::Point const *pps = &ps[0];
            int const nps = ps.size();
            if (thickness == CV_FILLED) {
                cv::fillPoly(*m, &pps, &nps, 1, v);
            }
            else {
                cv::polylines(*m, &pps, &nps, 1, true, v, thickness);
            }
        }
        virtual void bbox (cv::Rect_<float> *bb) const {
            float min_x = 1, min_y = 1;
            float max_x = 0, max_y = 0;
            for (auto const &p: points) {
                if (p.x < min_x) min_x = p.x;
                if (p.y < min_y) min_y = p.y;
                if (p.x > max_x) max_x = p.x;
                if (p.y > max_y) max_y = p.y;
            }
            float area = (max_x - min_x) * (max_y - min_y);
            if (area <= 0) {
                *bb = cv::Rect_<float>();
            }
            else {
                bb->x = min_x;
                bb->y = min_y;
                bb->width = max_x - min_x;
                bb->height = max_y - min_y;
            }
        }
        virtual void zoom (cv::Rect_<float> const &bb) {
            for (auto &p: points) {
                p.x -= bb.x;
                p.y -= bb.y;
                p.x /= bb.width;
                p.y /= bb.height;
            }
        }
    };

    std::shared_ptr<Shape> Shape::create (Json const &geo) {
        string type = geo["type"].string_value();
        if (type == "rect") {
            return std::shared_ptr<Shape>(new Box(geo["geometry"]));
        }
        if (type == "ellipse") {
            return std::shared_ptr<Shape>(new Ellipse(geo["geometry"]));
        }
        else if (type == "polygon") {
            return std::shared_ptr<Shape>(new Poly(geo["geometry"]));
        }
        CHECK(0) << "unknown shape: " << type;
        return 0;
    }

    Annotation::Annotation (string const &txt) {
        string err;
        Json json = Json::parse(txt, err);
        if (err.size()) {
            LOG(ERROR) << "Bad json: " << err << " (" << txt << ")";
            return;
        }
        for (auto const &x: json["shapes"].array_items()) {
            shapes.emplace_back(Shape::create(x));
        }
    }

    void Annotation::dump (string *str) const {
        vector<Json> ss;
        for (auto const &p: shapes) {
            ss.emplace_back();
            p->dump(&ss.back());
        }
        Json json = Json::object {{"shapes", std::move(ss)}};
        *str = json.dump();
    }

    void Annotation::draw (cv::Mat *m, cv::Scalar v, int thickness) const {
        for (auto const &p: shapes) {
            p->draw(m, v, thickness);
        }
    }

    void  Annotation::bbox (cv::Rect_<float> *bb) const { float min_x = 1, min_y = 1;
        float max_x = 0, max_y = 0;
        for (auto const &p: shapes) {
            cv::Rect_<float> sb;
            p->bbox(&sb);
            if (sb.area() <= 0) continue;

            if (sb.x < min_x) min_x = sb.x;
            if (sb.y < min_y) min_y = sb.y;

            sb.x += sb.width;
            sb.y += sb.height;

            if (sb.x > max_x) max_x = sb.x;
            if (sb.y > max_y) max_y = sb.y;
        }
        float area = (max_x - min_x) * (max_y - min_y);
        if (area <= 0) {
            *bb = cv::Rect_<float>();
        }
        else {
            bb->x = min_x;
            bb->y = min_y;
            bb->width = max_x - min_x;
            bb->height = max_y - min_y;
        }
    }

    void Annotation::zoom (cv::Rect_<float> const &bb) {
        for (auto &p: shapes) {
            p->zoom(bb);
        }
    }

    // for visualization only!!!
    void spectrogram_to_gray (cv::Mat m, cv::Mat *gray) {
        cv::Mat o(m.rows, m.cols, CV_32F);
        for (int i = 0; i < o.rows; ++i) {
            float const *in = m.ptr<float const>(i);
            float *out = o.ptr<float>(i);
            for (int j = 0; j < o.cols; ++j) {
                out[0] = std::sqrt(in[0] * in[0]
                                 + in[1] * in[1]);
                in += 2;
                out += 1;
            }
        }
        cv::normalize(o, o, 0, 255, cv::NORM_MINMAX, CV_8U);
        *gray = o;
    }

    // for visualization only!!!
    void spectrogram_to_bgr (cv::Mat m, cv::Mat *bgr) {
        float mm = 0;
        for (int i = 0; i < m.rows; ++i) {
            float const *in = m.ptr<float const>(i);
            for (int j = 0; j < m.cols; ++j) {
                float a = in[0];
                float b = in[1];
                float v = std::sqrt(a*a + b*b);
                if (v > mm) mm = v;
                in += 2;
            }
        }
        cv::Mat o(m.rows, m.cols, CV_8UC3);
        static const float BASE = 0.005;
        for (int i = 0; i < o.rows; ++i) {
            float const *in = m.ptr<float const>(i);
            uint8_t *out = o.ptr<uint8_t>(i);
            for (int j = 0; j < o.cols; ++j) {
                float a = in[0];
                float b = in[1];
                float v = std::sqrt(a*a + b*b)/mm;
                v = (log(std::max(BASE, v)) - log(BASE))/(-log(BASE)) ;
                out[0] = (std::atan2(b, a) + M_PI)/M_PI * 90;
                out[1] = 255;
                out[2] = BASE + (255-BASE) * v;
                in += 2;
                out += 3;
            }
        }
        cv::cvtColor(o, *bgr, CV_HSV2BGR);
    }

    void ImageLoader::load (RecordReader rr, PerturbVector const &p, Value *out,
           CacheValue *cache, std::mutex *mutex) const {
        Value cached;
        do {
            if (cache) { // access cache
                lock_guard lock(*mutex);
                cached = *cache;
            }
            if (cached.image.data) break;
            // cache miss, load the data
            Record r;
            rr(&r); // disk access
            cached.label = r.meta().label;
            //CHECK(r.size() >= (annotate ? 2 : 1));
            cached.image = decode_buffer(r.field(0), config.decode_mode);
            if ((config.channels > 0) && config.channels != cached.image.channels()) {
                cv::Mat tmp;
                if (cached.image.channels() == 3 && config.channels == 1) {
                    cv::cvtColor(cached.image, tmp, CV_BGR2GRAY);
                }
                else if (cached.image.channels() == 4 && config.channels == 1) {
                    cv::cvtColor(cached.image, tmp, CV_BGRA2GRAY);
                }
                else if (cached.image.channels() == 4 && config.channels == 3) {
                    cv::cvtColor(cached.image, tmp, CV_BGRA2BGR);
                }
                else if (cached.image.channels() == 1 && config.channels == 3) {
                    cv::cvtColor(cached.image, tmp, CV_GRAY2BGR);
                }
#ifdef SUPPORT_AUDIO_SPECTROGRAM
                else if (cached.image.type() == CV_32FC2 && config.channels == 1) {
                    spectrogram_to_gray(cached.image, &tmp);
                }
                else if (cached.image.type() == CV_32FC2 && config.channels == 3) {
                    spectrogram_to_bgr(cached.image, &tmp);
                }
#endif
                else CHECK(0) << "channel format not supported: from "
                              << cached.image.channels()
                              << " to " << config.channels;
                cached.image = tmp;
            }
            if (config.resize_width > 0 && config.resize_height > 0) {
                cv::resize(cached.image, cached.image, cv::Size(config.resize_width, config.resize_height), 0, 0);
            }
            else if (config.max_size > 0 || config.min_size > 0) {
                cv::Mat tmp;
                LimitSize(cached.image, config.min_size, config.max_size, &tmp);
                cached.image = tmp;
            }
            if (annotate == ANNOTATE_IMAGE) {
                if (r.size() < 2) {
                    cached.annotation = cv::Mat(cached.image.size(), CV_8U, cv::Scalar(0));
                }
                else {
                    auto anbuf = r.field(1);
                    cached.annotation = cv::imdecode(cv::Mat(1, boost::asio::buffer_size(anbuf), CV_8U,
                                const_cast<void *>(boost::asio::buffer_cast<void const *>(anbuf))),
                                cv::IMREAD_UNCHANGED);
                    if (!cached.annotation.data) {
                        cached.annotation = decode_raw(boost::asio::buffer_cast<char const *>(anbuf),
                                            boost::asio::buffer_size(anbuf));
                    }
                    if (cached.annotation.size() != cached.image.size()) {
                        cv::resize(cached.annotation, cached.annotation, cached.image.size(), 0, 0, cv::INTER_NEAREST);
                    }
                }
                CHECK(config.anno_min_ratio == 0) << "Not supported";
            }
            else if (annotate == ANNOTATE_JSON) {
                cv::Mat anno;
                if (config.anno_copy) {
                    anno = cached.image.clone();
                }
                else {
                    anno = cv::Mat(cached.image.size(), config.anno_type, cv::Scalar(0));
                }
                if (r.size() > 1) {
                    Annotation a(r.field_string(1));
                    cv::Scalar color(config.anno_color1,
                                 config.anno_color2,
                                 config.anno_color3);
                    a.draw(&anno, color, config.anno_thickness);

                    // we might want to perturb the cropping
                    // this might not be the perfect location either
                    do {
                        if (!(config.anno_min_ratio > 0)) break;
                        if (!(anno.total() > 0)) break;
                        cv::Rect_<float> fbb;
                        a.bbox(&fbb);
                        if (!(fbb.area() > 0)) break;
                        fbb.x *= anno.cols;
                        fbb.width *= anno.cols;
                        fbb.y *= anno.rows;
                        fbb.height *= anno.rows;

                        float min_roi_size = anno.total() * config.anno_min_ratio;
                        float roi_size = fbb.area();
                        if (roi_size >= min_roi_size) break;
                        float rate = std::sqrt(roi_size / min_roi_size);
                        int width = std::round(anno.cols * rate);
                        int height = std::round(anno.rows * rate);
                        cv::Rect bbox;
                        bbox.x = std::round(fbb.x);
                        bbox.y = std::round(fbb.y);
                        bbox.width = std::round(fbb.width);
                        bbox.height = std::round(fbb.height);
                        int dx = (width - bbox.width)/2;
                        int dy = (height - bbox.height)/2;
                        bbox.x -= dx;
                        bbox.y -= dy;
                        bbox.width = width;
                        bbox.height = height;
                        if (bbox.x < 0) {
                            bbox.x = 0;
                        }
                        if (bbox.y < 0) {
                            bbox.y = 0;
                        }
                        if (bbox.x + bbox.width > anno.cols) {
                            bbox.x = anno.cols - bbox.width;
                        }
                        if (bbox.y + bbox.height > anno.rows) {
                            bbox.y = anno.rows - bbox.height;
                        }
                        cv::Mat im = cached.image(bbox);
                        cv::Mat an = anno(bbox);
                        cv::Mat rim, ran;
                        cv::resize(im, rim, cached.image.size());
                        cv::resize(an, ran, anno.size());
                        cached.image = rim;
                        anno = ran;
                    } while (false);

                }
                cached.annotation = anno;
            }

            if (cache) {
                // write to cache
                lock_guard lock(*mutex);
                *cache = cached;
            }
        } while (false);

        if (!config.perturb) {
            *out = cached;
            if ((config.crop_width > 0) && (config.crop_height > 0)) {
                CHECK(out->image.cols >= config.crop_width);
                CHECK(out->image.rows >= config.crop_height);
                // cropping
                int marginx = out->image.cols - config.crop_width;
                int marginy = out->image.rows - config.crop_height;
                cv::Rect roi(marginx / 2,
                             marginy / 2,
                             config.crop_width,
                             config.crop_height);
                out->image = out->image(roi);
                if (out->annotation.data) {
                    out->annotation = out->annotation(roi);
                }
            }
            if (annotate == ANNOTATE_AUTO) {
                out->annotation = out->image;
            }
            return;
        }

        //float color, angle, scale, flip = false;
        //cv::Size sz = cached.image.size();
        cv::Mat image = cached.image, anno = cached.annotation;

        //TODO: scale might break min and/or max restriction
        if (p.angle != 0) {
            cv::Mat rot = cv::getRotationMatrix2D(cv::Point(image.cols/2, image.rows/2), p.angle, p.scale);
            {
                cv::Mat tmp;
                cv::warpAffine(image, tmp, rot, image.size(), cv::INTER_LINEAR, config.pert_border);
                //cv::resize(tmp, tmp, cv::Size(), p.scale, p.scale);
                image = tmp;
            }

            if (cached.annotation.data) {
                //cv::resize(anno, anno, cv::Size(), p.scale, p.scale, cv::INTER_NEAREST);
                cv::Mat tmp;
                cv::warpAffine(anno, tmp, rot, anno.size(), cv::INTER_NEAREST, config.pert_border); // cannot interpolate labels
                anno = tmp;
            }
        }
        else if (p.scale != 0) {
            {
                cv::Mat tmp;
                cv::resize(image, tmp, cv::Size(), p.scale, p.scale);
                image = tmp;
            }
            if (cached.annotation.data) {
                cv::Mat tmp;
                cv::resize(anno, tmp, cv::Size(), p.scale, p.scale, cv::INTER_NEAREST);
                anno = tmp;
            }
        }

        image += p.color;

        if (p.hflip && p.vflip) {
            cv::flip(image, out->image, -1);
            if (anno.data) {
                cv::flip(anno, out->annotation, -1);
            }
        }
        else if (p.hflip && !p.vflip) {
            cv::flip(image, out->image, 1);
            if (anno.data) {
                cv::flip(anno, out->annotation, 1);
            }
        }
        else if (!p.hflip && p.vflip) {
            cv::flip(image, out->image, 0);
            if (anno.data) {
                cv::flip(anno, out->annotation, 0);
            }
        }
        else {
            out->image = image;
            if (anno.data) {
                out->annotation = anno;
            }
        }
        if ((config.crop_width > 0) && (config.crop_height > 0)
            && ((out->image.cols > config.crop_width)
            || (out->image.rows > config.crop_height))) {
            // cropping
            int marginx = out->image.cols - config.crop_width;
            int marginy = out->image.rows - config.crop_height;
            cv::Rect roi(p.shiftx % marginx,
                         p.shifty % marginy,
                         config.crop_width,
                         config.crop_height);
            out->image = out->image(roi);
            if (out->annotation.data) {
                out->annotation = out->annotation(roi);
            }
        }
#if 0
        if (out->annotation.data) {
            // replicate positive region
            CHECK(out->annotation.type() == CV_8UC1);
            cv::Rect roi;
            cv::Rect target = roi;
            bound(out->annotation, &roi, 0.98);
            for (unsigned i = 0; i < 10; ++i) {
                for (;;) {
                    target.x = rand() % (out->image.cols - roi.width);
                    target.y = rand() % (out->image.rows - roi.height);
                    if ((target & roi).area() == 0) break;
                }
                out->image(target) = out->image(roi);
                out->annotation(target) = out->annotation(roi);
            }

        }
#endif
        if (annotate == ANNOTATE_AUTO) {
            out->annotation = out->image;
        }
        out->label = cached.label;
    }

    /*
    int prod (vector<int> const &v) {
        int r = 1;
        for (auto const &x: v) {
            r *= x;
        }
        return r;
    }
    */

    void ImageEncoder::encode (cv::Mat const &image, string *data) {
        if (code == "raw") {
            encode_raw(image, data);
            return;
        }
        std::vector<uint8_t> buffer;
        cv::imencode(code.empty() ? ".jpg": code, image, buffer);
        char const *from = reinterpret_cast<char const *>(&buffer[0]);
        *data = string(from, from + buffer.size());
    }

    void ImageReader::read (fs::path const &path, string *data) {
        bool do_code = code.size() || (mode != cv::IMREAD_UNCHANGED);
        cv::Mat image = cv::imread(path.native(), mode);
        if (!image.data) { // try raw
            string buf;
            fs::ifstream is(path, std::ios::binary);
            if (!is) throw BadFile(path);
            is.seekg(0, std::ios::end);
            if (!is) throw BadFile(path);
            buf.resize(is.tellg());
            is.seekg(0);
            is.read(&buf[0], buf.size());
            if (!is) throw BadFile(path);
            image = decode_raw(&buf[0], buf.size());
        }
        if (!image.data) throw BadFile(path);
        if (resize > 0) {
            cv::resize(image, image, cv::Size(resize, resize));
            do_code = true;
        }
        else if (max > 0) {
            cv::Mat rs;
            LimitSize(image, max, &rs);
            if (rs.total() != image.total()) {
                image = rs;
                do_code = true;
            }
        }
        if (do_code) {
            encode(image, data);
        }
        else {
            // read original file
            uintmax_t sz = fs::file_size(path);
            data->resize(sz);
            fs::ifstream is(path, std::ios::binary);
            is.read(&data->at(0), data->size());
            if (!is) throw BadFile(path);
        }
    }

    void encode_raw (cv::Mat m, string *s) {
        std::ostringstream ss;
        int type = m.type();
        int rows = m.rows;
        int cols = m.cols;
        int elemSize = m.elemSize();
        ss.write(reinterpret_cast<char const *>(&type), sizeof(type));
        ss.write(reinterpret_cast<char const *>(&rows), sizeof(rows));
        ss.write(reinterpret_cast<char const *>(&cols), sizeof(cols));
        ss.write(reinterpret_cast<char const *>(&elemSize), sizeof(elemSize));
        for (int i = 0; i < rows; ++i) {
            ss.write(m.ptr<char const>(i), cols * elemSize);
        }
        *s = ss.str();
    }

    cv::Mat decode_raw (char const *buf, size_t sz) {
        if (sz < sizeof(int) * 4) return cv::Mat();
        int type = *reinterpret_cast<int const *>(buf); buf += sizeof(int); sz -= sizeof(int);
        int rows = *reinterpret_cast<int const *>(buf); buf += sizeof(int); sz -= sizeof(int);
        int cols = *reinterpret_cast<int const *>(buf); buf += sizeof(int); sz -= sizeof(int);
        int elemSize = *reinterpret_cast<int const *>(buf); buf += sizeof(int); sz -= sizeof(int);
        if (sz != rows * cols * elemSize) return cv::Mat();
        cv::Mat m(rows, cols, type);
        if (m.elemSize() != elemSize) return cv::Mat();
        size_t line = cols * elemSize;
        for (int i = 0; i < rows; ++i) {
            std::copy(buf, buf + line, m.ptr<char>(i));
            buf += line;
        }
        return m;
    }

    cv::Mat decode_buffer (const_buffer imbuf, int mode) {
        cv::Mat image = cv::imdecode(cv::Mat(1, boost::asio::buffer_size(imbuf), CV_8U,
                        const_cast<void *>(boost::asio::buffer_cast<void const *>(imbuf))),
                        mode);
        if (!image.data) {
            image = decode_raw(boost::asio::buffer_cast<char const *>(imbuf),
                        boost::asio::buffer_size(imbuf));
        }
        return image;
    }

}


#include "KinectDataConverter.h"

#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/round.hpp>

namespace hpe
{
    typedef union
    {
        struct
        {
            unsigned char Blue;
            unsigned char Green;
            unsigned char Red;
            unsigned char Alpha;
        };
        float float_value;
        uint32_t long_value;
    } RGBValue;

    KinectDataConverter::KinectDataConverter()
    {
    }

    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr KinectDataConverter::DepthRGBToPointCloud(cv::Mat &depth, cv::Mat &bgr)
    {
        cv::Size depthImageSize = depth.size();

        float centerX, centerY;
        float scaleFactorX, scaleFactorY;

        ComputeCameraIntrinsics(depthImageSize, centerX, centerY, scaleFactorX, scaleFactorY);

        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr result(new pcl::PointCloud<pcl::PointXYZRGBA>);
        result->width = depthImageSize.width;
        result->height = depthImageSize.height;
        result->is_dense = false;
        result->points.resize(depthImageSize.width * depthImageSize.height);

        for (int y = 0; y < depthImageSize.height; ++y)
        {
            for (int x = 0; x < depthImageSize.width; ++x)
            {
                pcl::PointXYZRGBA &pt = result->at(x, y);

                float depthValue = (float) depth.at<float>(y, x);

                if (depthValue == -1000.f || depthValue == 0.0f || boost::math::isnan(depthValue))
                {
                    pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
                }
                else
                {
                    //depthValue /= 1000;
                    pt.x = (static_cast<float>(x) - centerX) * scaleFactorX * depthValue;
                    pt.y = (centerY - static_cast<float>(y)) * scaleFactorY * depthValue;
                    pt.z = depthValue;

                    cv::Vec3b colorValue = bgr.at<cv::Vec3b>(y, x);
                    RGBValue color;
                    color.Red = colorValue[2];
                    color.Green = colorValue[1];
                    color.Blue = colorValue[0];
                    pt.rgba = color.long_value;
                }
            }
        }

        return result;
    }

    void KinectDataConverter::ComputeCameraIntrinsics(cv::Size &imageSize, float &centerX, float &centerY, float &scaleFactorX, float &scaleFactorY)
    {
        int dims[] = {imageSize.width, imageSize.height};

        // The 525 factor default is only true for VGA. If not, we should scale
        scaleFactorX = scaleFactorY = 1 / 525.f * 640.f / dims[0];
        centerX = ((float)dims[0] - 1.f) / 2.f;
        centerY = ((float)dims[1] - 1.f) / 2.f;
    }

    pcl::PointXYZ KinectDataConverter::ConvertOnePoint(float x, float y, float depthValue, cv::Size frameSize)
    {
        float centerX, centerY;
        float scaleFactorX, scaleFactorY;

        ComputeCameraIntrinsics(frameSize, centerX, centerY, scaleFactorX, scaleFactorY);

        pcl::PointXYZ pt;
        pt.x = (static_cast<float>(x) - centerX) * scaleFactorX * depthValue;
        pt.y = (centerY - static_cast<float>(y)) * scaleFactorY * depthValue;
        pt.z = depthValue;
        return pt;
    }

}


#include <iostream>
#include <cv.h>
#include <highgui.h>
#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_io.hpp>
#include <time.h>

#include "AppRecog.h"

using namespace std;
using namespace cv;

extern unsigned long current_utime();

int main (int argc, char** argv) {
    // char* imgfile = argv[1];
    // Mat srcimg = imread(imgfile, 3);

    // Vec3f circle;
    // Mat gray_image;

    // cv::Mat dstimg;
    // cv::RotatedRect match_rect;
    // cv::Vec3f match_circle;

    // cv::Mat cameraMat;
    // cv::Mat distCoeffs;

    // std::string calib_file = "../script/camera_calib_lhand.yaml";
    // std::cout << "calib_file: " << calib_file << std::endl;

    // int width,height;
    // cv::FileStorage cvfs(calib_file, CV_STORAGE_READ);
    // cv::FileNode node(cvfs.fs, NULL); // Get Top Node
    // width = node["image_width"];
    // height = node["image_height"];
    // cv::FileNode fn = node[string("camera_matrix")];
    // cv::read(fn[0], cameraMat);
    // cv::read(fn[1], distCoeffs);

    // cv::Mat rotation, translation;
    // //mi_match_proc(srcimg, dstimg, match_rect, match_circle, 1, cameraMat, distCoeffs, "distance_transform", 7);
    // recognize(srcimg, dstimg, "partsA", cameraMat, distCoeffs, rotation, translation, "canny", true);

    // imshow("dst", dstimg);
    // waitKey(0);

    // cerr << "finished" << endl;
    return 0;
}


/***************************************************************************
 * Copyright 1998-2020 by authors (see AUTHORS.txt)                        *
 *                                                                         *
 *   This file is part of LuxCoreRender.                                   *
 *                                                                         *
 * Licensed under the Apache License, Version 2.0 (the "License");         *
 * you may not use this file except in compliance with the License.        *
 * You may obtain a copy of the License at                                 *
 *                                                                         *
 *     http://www.apache.org/licenses/LICENSE-2.0                          *
 *                                                                         *
 * Unless required by applicable law or agreed to in writing, software     *
 * distributed under the License is distributed on an "AS IS" BASIS,       *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.*
 * See the License for the specific language governing permissions and     *
 * limitations under the License.                                          *
 ***************************************************************************/

#include <boost/format.hpp>

#include "slg/bsdf/bsdf.h"
#include "slg/lights/mapspherelight.h"

using namespace std;
using namespace luxrays;
using namespace slg;

//------------------------------------------------------------------------------
// MapSphereLight
//------------------------------------------------------------------------------

MapSphereLight::MapSphereLight() : imageMap(NULL), func(NULL) {
}

MapSphereLight::~MapSphereLight() {
	delete func;
}

void MapSphereLight::Preprocess() {
	SphereLight::Preprocess();

	delete func;
	func = new SampleableSphericalFunction(new ImageMapSphericalFunction(imageMap));
}

void MapSphereLight::GetPreprocessedData(float *localPosData, float *absolutePosData,
		float *emittedFactorData, const SampleableSphericalFunction **funcData) const {
	SphereLight::GetPreprocessedData(localPosData, absolutePosData, emittedFactorData);

	if (funcData)
		*funcData = func;
}

float MapSphereLight::GetPower(const Scene &scene) const {
	return imageMap->GetSpectrumMeanY() * SphereLight::GetPower(scene);
}

Spectrum MapSphereLight::Emit(const Scene &scene,
		const float time, const float u0, const float u1,
		const float u2, const float u3, const float passThroughEvent,
		Ray &ray, float &emissionPdfW,
		float *directPdfA, float *cosThetaAtLight) const {
	// I sample as SphereLight::Emit() instead of func->Sample() because I would
	// have to reject half of the samples due to emitting below the sphere surface
	const Spectrum result = SphereLight::Emit(scene,
			time, u0, u1, u2, u3, passThroughEvent,
			ray, emissionPdfW,
			directPdfA, cosThetaAtLight);

	const Vector localFromLight = Normalize(Inverse(lightToWorld) * ray.d);

	return result *	((SphericalFunction *)func)->Evaluate(localFromLight) /
			func->Average();
}

Spectrum MapSphereLight::Illuminate(const Scene &scene, const BSDF &bsdf,
		const float time, const float u0, const float u1, const float passThroughEvent,
        Ray &shadowRay, float &directPdfW,
		float *emissionPdfW, float *cosThetaAtLight) const {
	const Spectrum result = SphereLight::Illuminate(scene, bsdf, time, u0, u1,
			passThroughEvent, shadowRay, directPdfW,
			emissionPdfW, cosThetaAtLight);

	const Vector localFromLight = Normalize(Inverse(lightToWorld) * (-shadowRay.d));
	const float funcPdf = func->Pdf(localFromLight);
	if (funcPdf == 0.f)
		return Spectrum();

	return result * ((SphericalFunction *)func)->Evaluate(localFromLight) / func->Average();
}

Properties MapSphereLight::ToProperties(const ImageMapCache &imgMapCache, const bool useRealFileName) const {
	const string prefix = "scene.lights." + GetName();
	Properties props = SphereLight::ToProperties(imgMapCache, useRealFileName);

	props.Set(Property(prefix + ".type")("mapsphere"));
	const string fileName = useRealFileName ?
		imageMap->GetName() : imgMapCache.GetSequenceFileName(imageMap);
	props.Set(Property(prefix + ".mapfile")(fileName));
	props.Set(imageMap->ToProperties(prefix, false));

	return props;
}


//          Copyright Boston University SESA Group 2013 - 2014.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)
#include "PageAllocator.h"

#include <mutex>

#include <boost/container/static_vector.hpp>

#include "../Align.h"
#include "Cpu.h"
#include "Debug.h"
#include "EarlyPageAllocator.h"
#include "MemMap.h"

ebbrt::ExplicitlyConstructed<boost::container::static_vector<
    ebbrt::PageAllocator, ebbrt::numa::kMaxNodes>>
    ebbrt::PageAllocator::allocators;

void ebbrt::PageAllocator::Init() {
  allocators.construct();
  for (unsigned i = 0; i < numa::nodes->size(); ++i) {
    allocators->emplace_back(Nid(i));
  }

  early_page_allocator::ReleaseFreePages([](Pfn start, Pfn end, Nid nid) {
    kassert(end - start != 0);
    // compute largest power of two that fits in the range
    auto largest_pow2 =
        1 << (sizeof(unsigned int) * 8 - __builtin_clz(end - start) - 1);
    // find the divide at which point the pages decrease in size
    auto divide = align::Up(start.val(), largest_pow2);
    auto pfn = start;
    // Add all the pages up to the divide
    while (pfn.val() != divide) {
      kassert(pfn.val() < divide);
      size_t order = __builtin_ctz(pfn.val());
      order = order > kMaxOrder ? kMaxOrder : order;
      EarlyFreePage(pfn, order, nid);
      pfn += 1 << order;
    }
    // now add the rest
    pfn = Pfn(divide);
    while (pfn != end) {
      kassert(pfn < end);
      auto order = sizeof(unsigned int) * 8 - __builtin_clz(end - pfn) - 1;
      order = order > kMaxOrder ? kMaxOrder : order;
      EarlyFreePage(pfn, order, nid);
      pfn += 1 << order;
    }
  });
#ifdef PAGE_CHECKER
  for (unsigned i = 0; i < numa::nodes->size(); ++i) {
    kassert((*allocators)[i].Validate());
  }
#endif
}

void ebbrt::PageAllocator::EarlyFreePage(Pfn start, size_t order, Nid nid) {
  kassert(order <= kMaxOrder);
  auto entry = PfnToFreePage(start);
  (*allocators)[nid.val()].free_page_lists[order].push_front(*entry);
  auto page = mem_map::PfnToPage(start);
  kassert(page != nullptr);
  page->usage = mem_map::Page::Usage::kPageAllocator;
  page->data.order = order;
}

ebbrt::PageAllocator& ebbrt::PageAllocator::HandleFault(EbbId id) {
  kassert(id == kPageAllocatorId);
  auto& allocator = (*allocators)[Cpu::GetMyNode().val()];
  EbbRef<PageAllocator>::CacheRef(id, allocator);
  return allocator;
}

ebbrt::PageAllocator::PageAllocator(Nid nid) : nid_(nid) {}

ebbrt::Pfn ebbrt::PageAllocator::AllocLocal(size_t order, uint64_t max_addr) {

  // cause a stack allocator page fault first, to
  // avoid deadlocking when calling malloc and
  // stack faulting at the same time
  int tmp;
  asm volatile("movl -1024(%%rsp), %0;" : "=r"(tmp) : :);

  std::lock_guard<SpinLock> lock(lock_);

  FreePage* fp = nullptr;
  auto this_order = order;
  while (this_order <= kMaxOrder) {
    auto& list = free_page_lists[this_order];
    auto it =
        std::find_if(list.begin(), list.end(), [max_addr](const FreePage& p) {
          return p.pfn().ToAddr() < max_addr;
        });
    if (it != list.end()) {
      fp = &*it;
      list.erase(it);
      break;
    }
    ++this_order;
  }
  if (fp == nullptr) {
    return Pfn::None();
  }

  while (this_order != order) {
    --this_order;
    FreePageNoCoalesce(fp->GetBuddy(this_order), this_order);
  }

  auto pfn = fp->pfn();
  auto page = mem_map::PfnToPage(fp->pfn());
  kassert(page != nullptr);
  page->usage = mem_map::Page::Usage::kInUse;
#ifdef PAGE_CHECKER
  kassert(AllocateAndCheck(pfn, order));
  kassert(Validate());
#endif
  return pfn;
}

ebbrt::Pfn ebbrt::PageAllocator::Alloc(size_t order, Nid nid,
                                       uint64_t max_addr) {
  if (nid == nid_) {
    return AllocLocal(order, max_addr);
  } else {
    return (*allocators)[nid.val()].AllocLocal(order, max_addr);
  }
}

void ebbrt::PageAllocator::FreePageNoCoalesce(Pfn pfn, size_t order) {
  auto entry = PfnToFreePage(pfn);
  free_page_lists[order].push_front(*entry);
  auto page = mem_map::PfnToPage(pfn);
  kassert(page != nullptr);
  page->usage = mem_map::Page::Usage::kPageAllocator;
  page->data.order = order;
}

void ebbrt::PageAllocator::Free(Pfn pfn, size_t order) {
  std::lock_guard<SpinLock> lock(lock_);
#ifdef PAGE_CHECKER
  kassert(Release(pfn, order));
#endif
  kassert(order <= kMaxOrder);
  while (order < kMaxOrder) {
    auto buddy = PfnToBuddy(pfn, order);
    auto page = mem_map::PfnToPage(buddy);
    if (page == nullptr ||
        page->usage != mem_map::Page::Usage::kPageAllocator ||
        page->data.order != order) {
      break;
    }
    // Buddy is free, remove it from the buddy system
    auto entry = reinterpret_cast<FreePage*>(buddy.ToAddr());
    auto it = free_page_lists[order].iterator_to(*entry);
    free_page_lists[order].erase(it);
    order++;
    pfn = std::min(pfn, buddy);
  }
  FreePageNoCoalesce(pfn, order);
#ifdef PAGE_CHECKER
  kassert(Validate());
#endif
}

#ifdef PAGE_CHECKER

bool ebbrt::PageAllocator::Validate() const {
  for (size_t i = 0; i <= kMaxOrder; ++i) {
    const auto& list = free_page_lists[i];
    for (const auto& free_page : list) {
      auto pfn = free_page.pfn();
      auto page = mem_map::PfnToPage(pfn);
      if (page->usage != mem_map::Page::Usage::kPageAllocator)
        return false;
      if (page->data.order != i) {
        return false;
      }
      // Check that no other page in this range is in the buddy allocator
      for (size_t j = 0; j <= kMaxOrder; ++j) {
        const auto& list = free_page_lists[j];
        for (const auto& free_page : list) {
          auto other_pfn = free_page.pfn();
          if (!(pfn == other_pfn && i == j) &&
              ((pfn <= other_pfn && other_pfn < (pfn + (1 << i))) ||
               (other_pfn <= pfn && pfn < (other_pfn + (1 << j))))) {
            return false;
          }
        }
      }
    }
  }
  return true;
}

bool ebbrt::PageAllocator::AllocateAndCheck(Pfn pfn, size_t order) {
  Allocation allocation;
  allocation.pfn = pfn;
  allocation.npages = 1 << order;
  auto it =
      std::lower_bound(allocations_.begin(), allocations_.end(), allocation);
  if (it != allocations_.end() && !(allocation < *it)) {
    return false;
  }
  allocations_.insert(it, std::move(allocation));
  return true;
}

bool ebbrt::PageAllocator::Release(Pfn pfn, size_t order) {
  Allocation allocation;
  allocation.pfn = pfn;
  allocation.npages = 1 << order;
  auto it = std::find(allocations_.begin(), allocations_.end(), allocation);
  if (it == allocations_.end()) {
    return false;
  }
  allocations_.erase(it);
  return true;
}
#endif


//
// Created by Karine Miras on 21/03/2017.
//

#include <algorithm>
#include <ctime>
#include <fstream>
#include <iostream>
#include <map>
#include <math.h>
#include <random>
#include <string>
#include <sstream>
#include <thread>
#include <vector>

#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>

//#include <mlpack/methods/neighbor_search/neighbor_search.hpp>
//
//using namespace mlpack;
//using namespace mlpack::neighbor; // NeighborSearch and NearestNeighborSort
//using namespace mlpack::metric; // EuclideanDistance

#include "Aux.h"
#include "Evolution.h"
#include "Genome.h"
#include "LSystem.h"
#include "Measures.h"

void Evolution::initializer()
{
    measures_names = std::map< std::string, std::string >();
    params = std::map<std::string, double>();
    morphological_grid_generation = std::map<std::string, std::vector<double>>();
    morphological_grid_accumulated = std::map<std::string, std::vector<std::string>>();
    population =  std::vector<Genome>();
    offspring =  std::vector<Genome>();
    archive = std::vector<Genome> ();
}

/**
 * Reads parameters from file.
 **/
void Evolution::readParams()
{
  std::string line;
  std::ifstream myfile(this->path+"lsystem/configuration.txt");

  /*   pop_size: integer
       num_generations: integer, local experiments
       k_neighbors: integer, for novelty search
       mutation_prob: 0-1, probability
       num_initial_comp: integer, number of initial (random) components in the production rules of the grammar
       prob_add_archive: 0-1, probability of a genome being added to the novelty archive
       replacement_iterations: integer, number of replacement iterations for the l-system
       offspring_prop: 0-1, proportion of the population size to dcalculate size of offspring
       max_comps: integer, maximum number of components allowed per phenotype
       grid_bins: integer, number of bins to break morphological space
       logs_to_screen: if exports the logs to the screen (1) or not (0)
       logs_to_file: if exports logs to a file (1) or not (0)
       show_phenotypes: flag to show the phenotype graphic
       export_phenotypes: if exports the phenotypes to images (1) or not (0)
       export_genomes: if exports the genomes to files (1) or not (0)
       size_component: size of each component in pixels
       spacing: spacing between components in pixels
       oscillator_min: integer, minimum for oscillator paramters
       oscillator_max: integer, maximum for oscillator paramters
       max_attempt_multi: maximum number of tournaments for multiobjetive
       objective:  *1) for single and (2) for multi
       num_runs: 1, local experiments
       minimum_robot: if there is a minimum robot size (1), and (0) if not
       tournament_size: integer, for selection
       crossover_prob: 0-1, probability
       fitness: types of fitness (1) speed1, (2) speed2, (3) speed3, (4) novelty, (5) behavior novelty
       surv_selection: (1) tournaments, (2) PplusO
       loco_fitness: (1) plain ground, (2) hill
  */

  if (myfile.is_open())
  {
    while (getline(
        myfile,
        line))
    {
      std::vector< std::string > tokens;

      // parameters label and value separated by space
      boost::split(
          tokens,
          line,
          boost::is_any_of(" "));

      // first item is the label, second is the value
      this->params[tokens[0]] = std::stod(tokens[1]);
    }
    myfile.close();
  }

}

/*
 * Change value of parameter for an experiment.
 * */
void Evolution::updateParameter(
    std::string key,
    double value)
{

  this->params[key] = value;
}

/**
 * Loads parameters from saved state.
 **/
void Evolution::loadsParams()
{

  std::string line;
  std::ifstream myfile(
      this->path+"experiments/"
      + this->experiment_name +
      "/configuration.txt");

  if (myfile.is_open())
  {
    while (getline(
        myfile,
        line))
    {

      std::vector< std::string > tokens;
      // parameters label and value separated by space
      boost::split(
          tokens,
          line,
          boost::is_any_of(" "));

      // first item is the label, second is the value
      this->params[tokens[0]] = std::stod(tokens[1]);
    }
    myfile.close();
  }
  else
  {
    std::cout<<"Unable to open parameters state."<<this->path+"experiments/"
      << this->experiment_name << "/configuration.txt"<<std::endl;
  }

}

/* Finds out in which generation the genome was generated.
 * @param idgenome - id of the genome for which to verify generation.
 * */
int Evolution::getGeneration_genome(std::string idgenome)
{

  int generation_genome = 0;
  int offspring_size =
      this->params["pop_size"] * this->params["offspring_prop"];

  // generation of the genome can be found by its id, considering the size of the population and the offspring
  if (this->params["offspring_prop"] == 1)
  {
    generation_genome = (int) trunc(
        std::stoi(idgenome)
        / this->params["pop_size"]) + 1;
  }
  else
  {
    generation_genome = (int) trunc((std::stoi(idgenome) - offspring_size)
                                    / offspring_size) + 1;
  }

  if (generation_genome == 0)
  { generation_genome = 1; }

  return generation_genome;

}

/**
*  Copies the phenotypes of the selection population to a separate folder.
*  @param generation - number of generation in evolution
**/
void Evolution::exportPop(int generation)
{

  for (int i = 0;
       i < this->population.size();
       i++)
  { // for each genome in the population

    // finds number of generation to which the genome belongs to
    int generation_genome = this->getGeneration_genome(this->population[i].getId());

    std::string filename = "/body_" + this->population[i].getId()
                           + "_p1_" +
                           this->population[i].getId_parent1() +
                           "_p2_" +
                           this->population[i].getId_parent2() +
                           ".png";

    std::string filename2 = "/brain_" + this->population[i].getId()+ ".png";

    std::string pathfrom = this->path+"experiments/"
                           + this->experiment_name + "/offspringpop" +
                           std::to_string(generation_genome);

    std::string pathto = this->path+"experiments/"
                         + this->experiment_name + "/selectedpop" +
                         std::to_string(generation);

    // copies phenotype file from offspring folder to selected population folder
    system(("exec cp " + pathfrom + filename + " " + pathto +
            filename).c_str());
    system(("exec cp " + pathfrom + filename2 + " " + pathto +
            filename2).c_str());

    // copies values of metrics to file of selected population
    std::string line;
    std::ifstream measures(
        this->path+"experiments/" + this->experiment_name +
        "/offspringpop" +
        std::to_string(generation_genome)
        + "/measures" +
        this->population[i].getId() + ".txt");

    while (getline(
        measures,
        line))
    {

      std::vector< std::string > tokens;
      boost::split(
          tokens,
          line,
          boost::is_any_of(":"));

    }
  }

}

/*
 * Compare phenotype of the individual with its parent's.
 * */
double Evolution::compareIndividual(
    Measures m,
    std::string idgenome)
{

  int generation_genome = this->getGeneration_genome(idgenome);

  std::string line;
  std::ifstream measures(
      this->path+"experiments/" + this->experiment_name +
      "/offspringpop" + std::to_string(generation_genome) +
      "/measures" + idgenome + ".txt");

  double dif = 0;
  while (getline(
      measures,
      line))
  {

    std::vector< std::string > tokens;
    boost::split(
        tokens,
        line,
        boost::is_any_of(":"));

    dif += std::pow(
        m.getGenome()->getMeasures()[tokens[0]] - std::stod(tokens[1]),
        2);
  }
  dif = roundf(std::sqrt(dif) * 100) / 100;

  return dif;

}

/*
 * Compare phenotype of the parents.
 * */
double Evolution::compareParents(
    std::string idparent1,
    std::string idparent2)
{

  int generation_genome_parent1 = this->getGeneration_genome(idparent1);
  std::string line;
  std::ifstream measures(
      this->path+"experiments/" + this->experiment_name +
      "/offspringpop" +
      std::to_string(generation_genome_parent1) +
      "/measures" + idparent1 + ".txt");

  int generation_genome_parent2 = this->getGeneration_genome(idparent2);
  std::string line2;
  std::ifstream measures2(
      this->path+"experiments/" + this->experiment_name +
      "/offspringpop" +
      std::to_string(generation_genome_parent2) +
      "/measures" + idparent2 + ".txt");

  double dif = 0;
  while (getline(
      measures,
      line))
  {

    getline(
        measures2,
        line2);

    std::vector< std::string > tokens, tokens2;
    boost::split(
        tokens,
        line,
        boost::is_any_of(":"));
    boost::split(
        tokens2,
        line2,
        boost::is_any_of(":"));

    dif += std::pow(
        std::stod(tokens[1]) - std::stod(tokens2[1]),
        2);
  }
  dif = roundf(std::sqrt(dif) * 100) / 100;

  return dif;

}

/**
 * Measures all the individuals of the population for several metrics.
 *  @param argc - command line parameter
 *  @param argv[] - command line parameter
 *  @param individuals - array with genomes
 *  @param dirpath - name of the output directory
 **/
void Evolution::measureIndividuals(
    int generation,
    std::vector< Genome > &individuals,
    std::string dirpath,
    int post_measuring)
{

      std::ofstream differences_file;
      std::string path =
          this->path+"experiments/" + this->experiment_name + "/distances.txt";
      differences_file.open(
          path,
          std::ofstream::app);

      // for each genome of the population
      for (int i = 0;
           i < individuals.size();
           i++)
      {

        Measures m = Measures(
            this->experiment_name,
            this->params,
            this->path
        );
        m.setGenome(individuals[i]);

        // measures phenotype
        m.measurePhenotype(
            this->params,
            dirpath,
            this->getGeneration_genome(individuals[i].getId()),
            post_measuring);

        // compares measures between individuals
          if(post_measuring == 0)
          {
              if (individuals[i].getId_parent1() != "N") {

                  double dif = this->compareIndividual(
                          m,
                          individuals[i].getId_parent1());
                  differences_file << individuals[i].getId() << " " << dif;

                  dif = this->compareIndividual(
                          m,
                          individuals[i].getId_parent2());
                  differences_file << " " << dif;

                  dif = this->compareParents(
                          individuals[i].getId_parent1(),
                          individuals[i].getId_parent2());
                  differences_file << " " << dif << std::endl;

              }
          }

        std::cout<<"Robot "<<individuals[i].getId()<<" has been measured."<<std::endl;

      }

      differences_file.close();
}

/**
 * Creates files of results containing headers.
 */

void Evolution::createHeader()
{

  std::ofstream file;

  std::string path =
      this->path+"experiments/" + this->experiment_name + "/history.txt";
  file.open(path);
  file
      << "generation idgenome noveltyfit locofit locoreal finalfit balfit "
          "novlfit penfit idparent1 idparent2 "
      << std::endl;
  file.close();

  path = this->path+"experiments/" + this->experiment_name + "/evolution.txt";
  file.open(path);
  file
      << "generation idbest_nov maxfit_nov meanfit_nov idbest_loco "
          "maxfit_loco meanfit_loco idbest_fin maxfit_fin meanfit_fin "
          "idbest_bal maxfit_bal meanfit_bal "
          "idbest_novl maxfit_novl meanfit_novl "
          "idbest_pen maxfit_pen meanfit_pen "
          "nichecoverage_generation nichecoverage_accumulated";
  file << std::endl;
  file.close();

  path = this->path+"experiments/" + this->experiment_name + "/measures.txt";
  file.open(path);
  file << "generation idgenome";
  for (const auto &it : this->measures_names)
  {
    file << " " << it.first;
  }
  file << std::endl;
  file.close();

  path = this->path+"experiments/" + this->experiment_name + "/distances.txt";
  file.open(path);
  file << "idgenome distances_parent1 distances_parent2 distances_parents"
       << std::endl;
  file.close();

}

void Evolution::saveHistory(int generation)
{

  for (int i = 0;
       i < this->population.size();
       i++)
  {

    std::ofstream history_file;
    std::string path =
        this->path + "experiments/" + this->experiment_name + "/history.txt";
    history_file.open(
        path,
        std::ofstream::app);

    history_file << std::to_string(generation) << " "
                 << this->population[i].getId() << " "
                 << this->population[i].getNoveltyFitness() << " "
                 << this->population[i].getLocomotionFitness() << " "
                 << this->population[i].getLocomotionReal() << " "
                 << this->population[i].getFinalFitness() << " "
                 << this->population[i].getBalanceFitness() << " "
                 << this->population[i].getNoveltyLocomotionFitness() << " "
                 << this->population[i].getPenaltyFitness() << " "
                 << this->population[i].getId_parent1() << " "
                 << this->population[i].getId_parent2() << " "
                 << std::endl;

    history_file.close();
  }
}

void Evolution::savesValidity(int generation)
{
  // saves list of robots and its validits to file to be read by simulator
  std::ofstream file;
  std::string path =
      this->path + "experiments/" + this->experiment_name +
      "/offspringpop" + std::to_string(generation) + "/validity_list.txt";
  file.open(path);
  for (int i = 0;
       i <  this->offspring.size();
       i++)
  {
    file << this->offspring[i].getId() << " " <<  this->offspring[i].getValid()
         << std::endl;
  }

    if(this->params["revaluate_parents"] == 1)
    {
        // todo: make a parameter for this generation progression later!
        if(generation == 11 or
           generation == 31 or
           generation == 61
           )
        {
            for (int i = 0;
                 i <  this->population.size();
                 i++)
            {
                file << this->population[i].getId() << " " <<  this->population[i].getValid()
                << std::endl;
            }
        }
    }

  file.close();
}

/**
 *
 * @return - the index of the winner genome
 */

int Evolution::tournament_single()
{

  std::random_device rd;
  std::default_random_engine generator(rd());
  std::uniform_int_distribution< int > dist_1(0, (int) this->population.size() - 1); // size of current pop (parents+offspring)

  int genome;
  int new_genome;
  double fitness;
  double new_fitness;

  for(int i=0; i<this->getParams()["tournament_size"]; i++)
  {
      new_genome = dist_1(generator);
      new_fitness = this->population[new_genome].getFinalFitness();

      if(i==0)
      {
          genome = new_genome;
          fitness = new_fitness;
      }else{
          if (new_fitness >= fitness)
          {
              genome = new_genome;
              fitness = new_fitness;
          }
      }
  }

  return genome;

}

int Evolution::tournament()
{
  if (this->getParams()["objective"] == 1){
    return this->tournament_single();
  }
  if (this->getParams()["objective"] == 2){
    return this->tournament_multi();
  }
}

int Evolution::tournament_multi()
{

  std::random_device rd;
  std::default_random_engine generator(rd());
  std::uniform_int_distribution< int > dist_1(0,
      (int) this->population.size() - 1); // size of current pop (parents+offspring)

  int genome1 = -1; // random genome 1
  int genome2 = -1; // random genome 2
  int dominant = -1;
  int attempts = 0;

  while( dominant == -1 and attempts <= this->getParams()["max_attempt_multi"] )
  {

    genome1 = dist_1(generator);
    genome2 = dist_1(generator);

    attempts = attempts + 1;

    // genome1 dominates
    if (this->population[genome1].getLocomotionFitness()
          >= this->population[genome2].getLocomotionFitness()
        and this->population[genome1].getNoveltyLocomotionFitness()
          >= this->population[genome2].getNoveltyLocomotionFitness()
           and
          (
              this->population[genome1].getLocomotionFitness()
                 > this->population[genome2].getLocomotionFitness()
              or this->population[genome1].getNoveltyLocomotionFitness()
                 > this->population[genome2].getNoveltyLocomotionFitness()

          )
        ) dominant = genome1;

    // genome2 dominates
    if (this->population[genome2].getLocomotionFitness()
            >= this->population[genome1].getLocomotionFitness()
        and this->population[genome2].getNoveltyLocomotionFitness()
            >= this->population[genome1].getNoveltyLocomotionFitness()

        and
          (
              this->population[genome2].getLocomotionFitness()
                 > this->population[genome1].getLocomotionFitness()
              or this->population[genome2].getNoveltyLocomotionFitness()
                 > this->population[genome1].getNoveltyLocomotionFitness()

          )
        ) dominant = genome2;
  }

  // if no dominent was found, chooses the first (random)
  if(dominant == -1) dominant = genome1;

  return dominant;

}

/**
*  Selection of genomes in a population.
**/
//
void Evolution::surv_selection_tournament()
{
  std::vector< Genome > selected = std::vector< Genome >();
  std::vector< int > index_selected = std::vector< int >();

  // selects genomes, maintaining population size
  for (int i = 0;
       i < this->params["pop_size"];
       i++)
  {

    int genome = -1;
    // selects one genome by tournament
    genome = this->tournament();

    // makes sure that the same genome wont be selected more than once
    while (std::find(
        index_selected.begin(),
        index_selected.end(),
        genome) != index_selected.end())
    {
      genome = this->tournament();
    }

    selected.push_back(this->population[genome]);
    index_selected.push_back(genome);

  }

  this->cleanMemory(index_selected);

  this->population = selected; // substitutes current population for the selected subset

  // # TEST: Tests if population size remains correct.
  this->tests.testPopsize(
      this->population,
      (int) this->params["pop_size"]);
}

void Evolution::surv_selection_comma()
{
    std::vector< int > index_selected = std::vector< int >();

    // cleans up only parents memory
    int aux_border = this->population.size()-this->offspring.size();
    for (int i = aux_border ; i < this->population.size(); i++)
    {
        index_selected.push_back(i);
    }
    this->cleanMemory(index_selected);

    this->population = this->offspring; // kills parents, offspring lives

    // # TEST: Tests if population size remains correct.
    this->tests.testPopsize(
                            this->population,
                            (int) this->params["pop_size"]);
}

/*
 * Deallocate memory used by the non-selected individuals.
 * */

void Evolution::cleanMemory(std::vector< int > index_selected)
{
  // cleaning memory
  std::cout<<"START cleaning memory for non-selected individuals"<<std::endl;
  for (int i = 0;
       i < this->population.size();
       i++)
  {
    // for non-selected individuals
    if(std::find(
        index_selected.begin(),
        index_selected.end(),
        i) == index_selected.end())
    {

      auto item = this->population[i].getGeneticString().getStart();
      while (item not_eq NULL)
      {
        auto item2 = item->next;
        delete item;
        item = item2;
      }

//       cleans grammar genetic-strings // PROBLEMATIC CLEANING THOUGH
//      for( auto &g: this->population[i].getGrammar())
//      {
//        item = g.second.getStart();
//        while (item not_eq NULL)
//        {
//          auto item2 = item->next;
//          delete item;
//          item = item2;
//        }
//      }

      this->cleanVertex(this->population[i].getDgs().getRoot());

    }
  }
  std::cout<<"FINISH cleaning memory for non-selected individuals"<<std::endl;
}

void Evolution::cleanVertex(DecodedGeneticString::Vertex * v){

  if(v != NULL)
  {
    this->cleanVertex(v->left);
    this->cleanVertex(v->front);
    this->cleanVertex(v->right);
    if(v->item == "C")
      this->cleanVertex(v->back);
    delete v;
  }
}

/**
 *  Saves state of the generations to file.
 */

void Evolution::exportGenerationMetrics(
    int generation,std::vector<int> metrics)
{
  std::ofstream evolution_file;
  std::string path =
      this->path+"experiments/" + this->experiment_name + "/evolution.txt";
  evolution_file.open(
      path,
      std::ofstream::app);

  evolution_file << generation;

  // fetches all types of fitness
  for(int f=0; f<6; f++)
  {
    double maximum_fitness;
    std::string best_genome = "0";
    double average_fitness = 0;
    double fitness = 0;

    for (int i = 0;
         i < this->getPopulation().size();
         i++)
    {
      if(f==0)
       fitness = this->getPopulation()[i].getNoveltyFitness();
      if(f==1)
        fitness = this->getPopulation()[i].getLocomotionFitness();
      if(f==2)
        fitness = this->getPopulation()[i].getFinalFitness();
      if(f==3)
        fitness = this->getPopulation()[i].getBalanceFitness();
      if(f==4)
        fitness = this->getPopulation()[i].getNoveltyLocomotionFitness();
      if(f==5)
        fitness = this->getPopulation()[i].getPenaltyFitness();

      // finds the maximum/best fitness of the population
      if(i==0)
      {
          maximum_fitness = fitness;
          best_genome = this->getPopulation()[0].getId();
      }else{
          if (fitness > maximum_fitness) {
              best_genome = this->getPopulation()[i].getId();
              maximum_fitness = fitness;
          }
      }
      average_fitness += fitness;
    }

    // calculates the average
    average_fitness /= this->getPopulation().size();

    evolution_file << " " << best_genome
                   << " " << maximum_fitness
                   << " " << average_fitness;
  }

  for (const auto &m : metrics)
  {
    evolution_file << " " << m;
  }

  evolution_file << std::endl;
  evolution_file.close();
}

void Evolution::setupEvolution()
{

  this->readParams();

  // cleans old files and creates folders for the experiment
  aux.removeFolder(this->path+"experiments/"+this->experiment_name);
  aux.createFolder(this->path+"experiments/"+this->experiment_name);

  // logs parameters configuration
  this->saveParameters();

}

void Evolution::postMeasuring()
{
    // loads state of parameters from previous experiment
    this->loadsParams();
    int argc = 1;  //
    char *argv[] = {"a"}; //

    LSystem LS(this->getParams());

    int total_genomes = this->params["pop_size"]
                        + (this->params["num_generations"]-1)
                          * this->params["pop_size"]
                          * this->params["offspring_prop"];

    for (int idgenome = 0; idgenome < total_genomes; idgenome++)
    {

        Genome gen = Genome(
                std::to_string(idgenome),
                  "",
                  "");

          // finds number of generation to which the genome belongs to
          int generation_genome = this->getGeneration_genome(std::to_string(idgenome));

          // reads the file with the genome
          std::ifstream listalphabet(
                  this->path + "experiments/" + this->experiment_name +
                  "/offspringpop" +
                  std::to_string(generation_genome) + "/genome" + std::to_string(idgenome) +
                  ".txt");
          std::string linealphabet;
          // for each letter of the alphabet
          while (getline(
                  listalphabet,
                  linealphabet))
          {

              // gets letter and production rule from file
              std::vector< std::string > items;
              boost::split(
                      items,
                      linealphabet,
                      boost::is_any_of(" "));
              std::vector< std::string > items_rule(
                      items.begin() + 1,
                      items.begin() + items.size() -
                      1);

              // build a genetic-string with the production rule for the letter
              auto lgs = GeneticString();
              lgs = gen.build_genetic_string(
                      lgs,
                      items_rule);

              // adds letter and its production rule (made a genetic-string) to the grammar of the genome
              gen.addLetterGrammar(
                      items[0],
                      lgs);

          }

        // adds genome to the population
       this->population.push_back(gen);

    }

      // develops genomes of the initial population
      this->developIndividuals(
              argc,
              argv,
              LS,
              0,
              this->population,
              this->experiment_name + "/offspringpop",
              1);

        std::ofstream file;
        std::string path = this->path+"experiments/" + this->experiment_name + "/measures_post.txt";
        file.open(path);
        file << "generation idgenome";
        for (const auto &it : this->measures_names)
        {
            file << " " << it.first;
        }
        file << std::endl;
        file.close();

      // measures phenotypes of the individuals
      this->measureIndividuals(
              0,
              this->population,
              "/offspringpop",
              1);

}

/**
*  Develops genomes of the population: 1- grows genetic-string of the genome according to grammar, 2- decodes it, 3- constructs the phenotype
*  @param argc - default argument
*  @param argv[] - default argument
*  @param LS - Lsystem structure containing the alphabet
*  @param individuals - array with genomes
**/
void Evolution::developIndividuals(
    int argc,
    char *argv[],
    LSystem LS,
    int generation,
    std::vector< Genome > &individuals,
    std::string dir,
    int post_measuring)
{
  // for each genome in the array
  for (size_t i = 0; i < individuals.size(); ++i)
  {
    // develops genome
    individuals[i].developGenomeIndirect(
        argc,
        argv,
        this->params,
        LS,
        generation,
        this->path+"experiments/"+dir,
        post_measuring);

    std::cout<<"Robot "<<individuals[i].getId()<<" has been late developed."<<std::endl;
  }
}

/**
 * Loads population of genomes from files, from previous experiment.
 **/
void Evolution::loadIndividuals(int generation, std::string type)
{

  std::string path_list = "";
  std::string folder = "";

  if(generation==1)
    folder = "offspringpop";
  else
    folder = "selectedpop";

  if(type == "population")
  {
    // generates list of files (genomes of last population)
    std::system(("ls " + this->path + "experiments/" + this->experiment_name +
                 "/"+folder + std::to_string(generation) +
                 ">" + this->path + "experiments/" + this->experiment_name +
                 "/temp.txt").c_str());

    path_list = this->path + "experiments/" +
                this->experiment_name +"/temp.txt";
  }
  else
  {
    // reads list of genomes in archive
    path_list = this->path+"experiments/" +
                this->experiment_name + "/archive.txt";
  }

  std::ifstream listgenomes(path_list.c_str());
  std::string linegenome;

  //for each file (genome)
  while (getline(
      listgenomes,
      linegenome))
  {

    std::vector< std::string > tokens;
    boost::split(
        tokens,
        linegenome,
        boost::is_any_of("_"));

    // if it is a body file for pop recovery, or archive recovery
    if(tokens[0] == "body" or type != "population")
    {
      std::string idgenome = "";
      std::string idparent1 = "";
      std::string idparent2 = "";

      if (type == "population")
      {
        boost::split(
            tokens,
            linegenome,
            boost::is_any_of("_."));
        idgenome = tokens[1];
        idparent1 = tokens[3];
        idparent2 = tokens[5];
      }
      else
      {
        boost::split(
            tokens,
            linegenome,
            boost::is_any_of(" "));
        idgenome = tokens[0];
        idparent1 = tokens[1];
        idparent2 = tokens[2];
      }

      Genome gen = Genome(
          idgenome,
          idparent1,
          idparent2);

      // finds number of generation to which the genome belongs to
      int generation_genome = this->getGeneration_genome(idgenome);

      // reads the file with the genome
      std::ifstream listalphabet(
          this->path + "experiments/" + this->experiment_name +
          "/offspringpop" +
          std::to_string(generation_genome) + "/genome" + idgenome +
          ".txt");
      std::string linealphabet;
      // for each letter of the alphabet
      while (getline(
          listalphabet,
          linealphabet))
      {

        // gets letter and production rule from file
        std::vector< std::string > items;
        boost::split(
            items,
            linealphabet,
            boost::is_any_of(" "));
        std::vector< std::string > items_rule(
            items.begin() + 1,
            items.begin() + items.size() -
            1);

        // build a genetic-string with the production rule for the letter
        auto lgs = GeneticString();
        lgs = gen.build_genetic_string(
            lgs,
            items_rule);

        // adds letter and its production rule (made a genetic-string) to the grammar of the genome
        gen.addLetterGrammar(
            items[0],
            lgs);

      }

      // reads the measures of the genome
      std::ifstream listmeasures(
          this->path + "experiments/" + this->experiment_name +
          "/offspringpop" +
          std::to_string(generation_genome) + "/measures" + idgenome +
          ".txt");
      std::string linemeasures;
      // for each measure of the list
      while (getline(
          listmeasures,
          linemeasures))
      {

        std::vector< std::string > tokens;
        boost::split(
            tokens,
            linemeasures,
            boost::is_any_of(":"));

        gen.updateMeasure(
            tokens[0],
            std::stod(tokens[1]));
      }

      // reads fitness of the genome
      std::ifstream locomotion(
          this->path + "experiments/" + this->experiment_name +
          "/offspringpop" +
          std::to_string(generation_genome) + "/locomotion_" + idgenome +
          ".txt");
      if (locomotion.is_open())
      {
        std::string linelocomotion;
        getline(
                locomotion,
            linelocomotion);

          std::vector< std::string > info;
          boost::split(
                  info,
                  linelocomotion,
                  boost::is_any_of(" "));

           gen.updateLocomotionFitness(std::stod(info[0]),
                                       std::stod(info[1]),
                                       std::stod(info[2]),
                                       std::stod(info[3]),
                                       this->params);
      }

      std::ifstream balance(
          this->path + "experiments/" + this->experiment_name +
          "/offspringpop" +
          std::to_string(generation_genome) + "/balance_" + idgenome +
          ".txt");
      if (balance.is_open())
      {
        std::string linefitness;
        getline(
                balance,
            linefitness);
          linefitness = linefitness.c_str();
          std::stringstream iss(linefitness);
          double d = 0;
          iss >> d;
          gen.updateBalanceFitness(d);
      }

      if (type == "population")
      {
        // adds genome to the population
        this->population.push_back(gen);
      }
      else
      {
        // adds genome to the archive
        this->archive.push_back(gen);
      }
    }

  }

};

/**
 *  Loads state of previous experiment.
 **/
int Evolution::loadExperiment()
{
  // loads state of parameters from previous experiment
  this->loadsParams();

  this->aux = Aux(
      this->experiment_name,
      this->getParams(),
      this->path);

  this->logsTime("start recovery gen");

  // loads generation number from previous  experiment
  int gi = std::stoi(this->readsEvolutionState()[0]);
  // loads next_id from previous experiment
  this->next_id = std::stoi(this->readsEvolutionState()[1]);

  // deletes possible remains of unfinished generation
  std::string pathdir =
      this->path+"experiments/" + this->experiment_name + "/selectedpop" +
      std::to_string(gi + 1);
  system(("exec rm -r " + pathdir).c_str());
  pathdir = this->path+"experiments/" + this->experiment_name + "/offspringpop" +
            std::to_string(gi + 1);
  system(("exec rm -r " + pathdir).c_str());

  std::cout<<"clean folders "<<std::endl;
  // loads  population and archive
  this->loadIndividuals(gi, "population");

  std::cout<<"load pop "<<std::endl;
  this->loadIndividuals(0, "archive");

  std::cout<<"clean archive "<<std::endl;
  // loads state of the morphological_grid_accumulated
  std::string line;
  std::ifstream myfile(
      this->path+"experiments/" + this->experiment_name +
      "/morphological_grid_accumulated.txt");
  while (getline(
      myfile,
      line))
  {
    std::vector< std::string > tokens, tokens2;
    boost::split(
        tokens,
        line,
        boost::is_any_of("-"));
    boost::split(
        tokens2,
        tokens[1],
        boost::is_any_of(" "));
    std::vector< std::string > tokens3(
        tokens2.begin(),
        tokens2.begin() + tokens2.size() -
        1);
    std::vector< std::string > points;
    for (int i = 0;
         i < tokens3.size();
         i++)
    {
      points.push_back(tokens3[i]);
    }
    this->morphological_grid_accumulated[tokens[0]] = points;
  }
  std::cout<<"load grid "<<std::endl;
  myfile.close();

  return gi;
}

/* Saves the fitness for genome.
 * */
void Evolution::saveLocomotionInfo(
    std::string genome_id,
    double x, double y, double z, double time)
{

  // updates locomotion fitness for the genome
  int genome_index = -1;
  int t_pop = -1;
  int generation_genome = -1;

    for (int i = 0;i < population.size();i++)
    {
        if(this->population[i].getId() == genome_id){
            genome_index = i;
            t_pop = 0;
        }
    }

    for (int i = 0;i < offspring.size();i++)
    {
        if(this->offspring[i].getId() == genome_id){
            genome_index = i;
            t_pop = 1;
        }
    }

    std::ofstream file;
    if(t_pop == 0)
    {
        this->population[genome_index].updateLocomotionFitness(x, y, z, time, this->params);
        generation_genome = this->getGeneration_genome(this->population[genome_index].getId());
    }

    if(t_pop == 1)
    {
        this->offspring[genome_index].updateLocomotionFitness(x, y, z, time, this->params);
        generation_genome = this->getGeneration_genome(this->offspring[genome_index].getId());

    }
    std::string path2 =
    this->path+"experiments/"
    + this->experiment_name + "/offspringpop" +
    std::to_string(generation_genome)+ "/locomotion_"+genome_id+".txt";
    file.open(path2);
    file
    << x <<" "<< y <<" "<< z <<" "<< time;
    file.close();

}

/* Saves the fitness for genome.
 * */
void Evolution::saveBalance(
    std::string genome_id,
    double balance)
{

    int genome_index = -1;
    int t_pop = -1;
    int generation_genome = -1;

    for (int i = 0;i < population.size();i++)
    {
        if(this->population[i].getId() == genome_id){
            genome_index = i;
            t_pop = 0;
        }
    }

    for (int i = 0;i < offspring.size();i++)
    {
        if(this->offspring[i].getId() == genome_id){
            genome_index = i;
            t_pop = 1;
        }
    }

     std::ofstream file;
    if(t_pop == 0)
    {
        this->population[genome_index].updateBalanceFitness(balance);
        generation_genome = this->getGeneration_genome(this->population[genome_index].getId());
    }

    if(t_pop == 1)
    {
      this->offspring[genome_index].updateBalanceFitness(balance);
      generation_genome = this->getGeneration_genome(this->offspring[genome_index].getId());
    }
    std::string path2 =
    this->path+"experiments/"
    + this->experiment_name + "/offspringpop" +
    std::to_string(generation_genome)+ "/balance_"+genome_id".txt";
    file.open(path2);
    file
    <<  balance;
    file.close();
}

/**
 * Tries to add individuals to an archive for NS.
 **/
void Evolution::addToArchive()
{
  std::random_device rd;
  // distribution for 0-1 probabilities
  std::default_random_engine generator(rd());
  std::uniform_real_distribution< double > prob(0.0, 1.0);

  std::ofstream file;
  std::string path =
      this->path+"experiments/" + this->experiment_name + "/archive.txt";
  file.open(path, std::ofstream::app);

  for (int i = 0; i < this->offspring.size(); i++)
  {
    // if raffled probability is within the constrained probability
    if (prob(generator) < this->getParams()["prob_add_archive"])
    {
      //copies object of the genome to archive
      this->archive.push_back(this->offspring[i]);

      file <<  this->offspring[i].getId()
           << " " << this->offspring[i].getId_parent1()
           << " " << this->offspring[i].getId_parent2()<< std::endl;
    }
  }

  file.close();
}

/* Calculate the novelty of the individuals.
 * */
void Evolution::calculateNovelty()
{
//  std::vector<Genome> individuals_compare;
//  individuals_compare.insert(individuals_compare.end(),
//                             this->population.begin(), this->population.end());
//  individuals_compare.insert(individuals_compare.end(),
//                             this->archive.begin(), this->archive.end());
//
//
//  std::map< std::string, double > measures;
//
//
//  //matrix with all individuals
//  // columns: number of metrics / lines: number of genomes
//  arma::mat compare(
//     // individuals_compare[0].getMeasures().size(),
//      9,
//      individuals_compare.size());
//
//
//  for (int i = 0; i < individuals_compare.size(); i++)
//  {
//    int m = 0;
//    for (const auto &it : individuals_compare[i].getMeasures())
//    {
//      if(it.second == 'branching'
//       or it.second == 'connectivity1'
//          or it.second == 'connectivity2'
//             or it.second == 'coverage'
//                or it.second == 'effective_joints'
//                   or it.second == 'length_ratio'
//                      or it.second == 'sensors'
//                         or it.second == 'symmetry'
//                            or it.second == 'total_components')
//      {
//        compare(
//            m,
//            i) = it.second;
//        m++;
//      }
//    }
//  }
//
//  for (int i = 0; i < this->population.size(); i++)
//  {
//    // matrix with individuals which will be compared to the others
//    // columns: number of metrics / single line: genome
//    arma::mat reference(
//        //this->population[0].getMeasures().size(),
//        9,
//        1);
//
//    int m = 0;
//    for (const auto &it : this->population[i].getMeasures())
//    {
//      if(it.second == 'branching'
//         or it.second == 'connectivity1'
//         or it.second == 'connectivity2'
//         or it.second == 'coverage'
//         or it.second == 'effective_joints'
//         or it.second == 'length_ratio'
//         or it.second == 'sensors'
//         or it.second == 'symmetry'
//         or it.second == 'total_components') {
//        reference(
//                m,
//                0) = it.second;
//        m++;
//      }
//    }
//
//    NeighborSearch< NearestNeighborSort, EuclideanDistance > nn(compare);
//    arma::Mat< size_t > neighbors;
//    arma::mat distances;
//
//    // search for each individual, the nearest neighbors (+1 because it includes itself)
//    nn.Search(
//        reference,
//        this->params["k_neighbors"] + 1,
//        neighbors,
//        distances);
//
//    double fitness = 0;
//    for (size_t j = 0; j < neighbors.n_elem; ++j)
//    {
//      fitness += distances[j];
//      this->aux.logs(
//          "nv nearest neighbor  " + std::to_string(j) + " for genome "
//          + this->population[i].getId() + " has distance "
//          + std::to_string(distances[j]));
//    }
//
//    // averages the nearest neighboards
//    fitness = fitness / this->params["k_neighbors"];
//
//    this->population[i].updateNoveltyFitness(fitness);
//
//  }
//    std::cout<<"Novelty has been calculated."<<std::endl;
}

void Evolution::calculateNoveltyLocomotion()
{
//  std::vector<Genome> individuals_compare;
//  individuals_compare.insert(individuals_compare.end(),
//                             this->population.begin(), this->population.end());
//  individuals_compare.insert(individuals_compare.end(),
//                             this->archive.begin(), this->archive.end());
//
//  //matrix with all individuals
//  // columns: number of metrics / lines: number of genomes
//  arma::mat compare(
//      1,
//      individuals_compare.size());
//
//  for (int i = 0; i < individuals_compare.size(); i++)
//  {
//    int m = 0;
//    compare(
//          m,
//          i) = individuals_compare[i].getLocomotionFitness();
//  }
//
//  for (int i = 0; i < this->population.size(); i++)
//  {
//    // matrix with individuals which will be compared to the others
//    // columns: number of metrics / single line: genome
//    arma::mat reference(
//        1,
//        1);
//
//    int m = 0;
//    reference(
//          m,
//          0) = this->population[i].getLocomotionFitness();
//
//
//    NeighborSearch< NearestNeighborSort, EuclideanDistance > nn(compare);
//    arma::Mat< size_t > neighbors;
//    arma::mat distances;
//
//    // search for each individual, the nearest neighbors (+1 because it includes itself)
//    nn.Search(
//        reference,
//        this->params["k_neighbors"] + 1,
//        neighbors,
//        distances);
//
//    double fitness = 0;
//    for (size_t j = 0; j < neighbors.n_elem; ++j)
//    {
//      fitness += distances[j];
//      this->aux.logs(
//          "nv2 nearest neighbor  " + std::to_string(j) + " for genome "
//          + this->population[i].getId() + " has distance "
//          + std::to_string(distances[j]));
//    }
//
//    // averages the nearest neighboards
//    fitness = fitness / this->params["k_neighbors"];
//
//    this->population[i].updateNoveltyLocomotionFitness(fitness);
//
//  }
//  std::cout<<"Novelty of locomotion has been calculated."<<std::endl;
}

/**
 * Consolidates final fitness for evolution.
 **/
void Evolution::calculateFinalFitness()
{

  double fitness;

 for (int i = 0; i < this->population.size(); i++)
  {

      if(this->params["fitness"] == 1) //s1
      {
          fitness = this->population[i].getLocomotionFitness();

          if(this->params["loco_fitness"] == 2 and this->population[i].getLocomotionFitness() == 0)
          {
              fitness = - 0.1;
          }
      }

      if(this->params["fitness"] == 2) // s2
      {
          fitness = this->population[i].getLocomotionFitness()
                  * this->population[i].getNoveltyFitness();

      }

      if(this->params["fitness"] == 3) // s3
      {
           fitness = this->population[i].getLocomotionFitness()
                      //  * this->population[i].getNoveltyFitness()
                        * this->population[i].getPenaltyFitness();

      }

      if(this->params["fitness"] == 4) // phenotypic novelty
      {
          fitness = this->population[i].getNoveltyFitness();
      }

      if(this->params["fitness"] == 5) // s1 novelty
      {
          fitness = this->population[i].getNoveltyLocomotionFitness();
      }

    this->population[i].updateFinalFitness(fitness);
  }

  std::cout<<"Final fitnesses have been calculated."<<std::endl;
}

void Evolution::calculatePenaltyFitness()
{
  for (int i = 0; i < this->population.size(); i++)
  {

    double fitness =
       // std::max(0.1, 1 - this->population[i].getMeasures()["connectivity2"]);
       std::max(0.1, 1 - this->population[i].getMeasures()["total_components"]);

    this->population[i].updatePenaltyFitness(fitness);
  }
  std::cout<<"Penalty Fitness have been calculated."<<std::endl;
}

/**
*  Evolution in the search for locomotion - part 1 of the process.
**/
double Evolution::runExperiment_part1(
    int generation, int load_experiment)
{
  int argc = 1;  //
  char *argv[] = {"a"}; //

  if(load_experiment == 1)
  {
    this->loadExperiment();
  }

  this->aux = Aux(
      this->experiment_name,
      this->getParams(),
      this->path);

  // loads alphabet with letters and commands
  LSystem LS(this->getParams());

  this->aux.logs("------------ generation " + std::to_string(generation) + " ------------");
  this->logsTime("start gen");

  this->aux.createFolder(this->path+"experiments/"+this->experiment_name +
                         "/offspringpop"+std::to_string(generation));

  if(generation == 1)
  {
    this->createHeader();

    // initializes population
    this->initPopulation(LS);
    std::cout<<"First population has been created."<<std::endl;
  }
  else{

    this->aux.createFolder(
        this->path+"experiments/"+this->experiment_name + "/selectedpop" + std::to_string(generation));

    this->offspring = std::vector< Genome >();

    // creates offspring
    this->crossover(LS);
    std::cout<<"Crossovers of generation "<<generation<<" have been realized."<<std::endl;

    // mutates new individuals
    this->mutation(LS);
    std::cout<<"Mutations of generation "<<generation<<" have been realized."<<std::endl;

  }

  // develops genomes of the initial population
  this->developIndividuals(
      argc,
      argv,
      LS,
      generation,
      this->offspring,
      this->experiment_name + "/offspringpop",
      0);

  // measures phenotypes of the individuals
  this->measureIndividuals(
      generation,
      this->offspring,
      "/offspringpop",
      0);

  // updates the average measures for the population
  this->savesValidity(generation);

}

/**
*  Evolution in the search for locomotion - part 2 of the process. After
 *  fitness evaluation.
**/
double Evolution::runExperiment_part2(int generation)
{

  // adds new individuals to population
  for (int j = 0;
       j < this->offspring.size();
       j++)
  {
    this->population.push_back(this->offspring[j]);
  }

    if(this->params["fitness"] == 2
       or this->params["fitness"] == 3
       or this->params["fitness"] == 4
       )
    {
        this->calculateNovelty();
    }

    if(this->params["fitness"] == 5 or this->params["fitness"] == 6)
    {
        this->calculateNoveltyLocomotion();
    }

    if(this->params["fitness"] == 3)
    {
        this->calculatePenaltyFitness();
    }

  this->calculateFinalFitness();

  // saves a history with the fitnesses of new individuals
  this->saveHistory(generation);

  std::vector< int > niche_measures = this->calculateNicheCoverage();

  if(generation != 1)
  {
    // selects individuals, keeping the population with a fixed size
    if(this->params["surv_selection"] == 1)
    {
      this->surv_selection_tournament();
    }
      if(this->params["surv_selection"] == 2)
      {
          this->surv_selection_comma();
      }

    std::cout<<"Selection of generation "<<generation<<" has been realized."<<std::endl;

    // saves phenotypes of the selected population to a separated folder (only for visualization issues)
    this->exportPop(generation);
  }

  // saves metrics of evolution to file
  this->exportGenerationMetrics(
      generation,niche_measures);

  this->summaryNicheCoverage();

  // adds some of the new individuals to archive
  // fitness in the archive might be outdated by it would be used
  this->addToArchive();

  // saves the number of the last generation created/evaluated
  this->writesEvolutionState(
      generation,
      this->next_id);

  this->logsTime("end gen");
}

void Evolution::summaryNicheCoverage()
{

  std::ofstream file;

  std::string path = this->path+"experiments/" + this->experiment_name +
                     "/morphological_grid_summary.txt";
  file.open(path);
  file << "point count"<< std::endl;

  for (const auto &it : this->morphological_grid_accumulated)
  {

    file << it.first + " " << it.second.size() << std::endl;
  }
  file.close();

}

std::vector< Genome > Evolution::getPopulation()
{
  return this->population;
}

std::map< std::string, double > Evolution::getParams()
{
  return params;
}

/*
 * Exports the parameters of the experiment.
 **/
void Evolution::saveParameters()
{

  std::ofstream param_file;
  std::string path =
      this->path+"experiments/" + this->experiment_name + "/configuration.txt";
  param_file.open(path);

  // writes each parameter to a different line in a the file
  for (auto &it : this->getParams())
  {

    param_file << it.first << " " << it.second;
    param_file << std::endl;
  }
  param_file.close();
}

/*
 * Logs time.
 **/
void Evolution::logsTime(std::string moment)
{

  time_t sta = time(0);
  char *dtsta = ctime(&sta);
  this->aux.logs("experiment " + moment + ": " + dtsta);

}

/*
 * Logs a reference of generation and genome for evolution to be recovered.
 * */
void Evolution::writesEvolutionState(
    int generation,
    int next_id)
{

  std::ofstream logs_file;
  std::string path = this->path+"experiments/" + this->experiment_name +
                     "/evolutionstate.txt";
  logs_file.open(path);
  logs_file << generation << " " << next_id;
  logs_file.close();
}

/*
 * Reads number of the generation from which the recovered evolution should start from.
 * */
std::vector< std::string > Evolution::readsEvolutionState()
{

  std::string line;
  std::ifstream myfile(
      this->path+"experiments/" + this->experiment_name +
      "/evolutionstate.txt");
  if (myfile.is_open())
  {

    getline(
        myfile,
        line);
    std::vector< std::string > tokens;
    // parameters label and value separated by space
    boost::split(
        tokens,
        line,
        boost::is_any_of(" "));

    return tokens;

  }
  else
  {
    this->aux.logs("Unable to open evolutionstate file.");
  }
  myfile.close();

}

/*
 * Calculates the quality metric for the novelty search: niche coverage and bins of measures
 * */
std::vector< int >
Evolution::calculateNicheCoverage()
{
  morphological_grid_generation =  std::map<std::string, std::vector<double>>();

  for (int i = 0;
       i < this->offspring.size();
       i++)
  {

    std::string key_point = "";
    double distance = 0;

    // for each measure (dimension)
    for (const auto &it : this->offspring[i].getMeasures())
    {
      // accounts for NC
      // for each bin
      for (int b = 1;
           b <= this->params["grid_bins"];
           b++)
      {
        // if value is zero, sets into the first bin
        if (it.second == 0 and b == 1)
        {

          key_point += std::to_string(b) + "|";
          distance +=
              -1 * (it.second - (b / this->params["grid_bins"]));
        }
        // otherwise, sets value for measure into the correct bin
        if (it.second > ((b - 1) / this->params["grid_bins"]) and
            it.second <= (b / this->params["grid_bins"]))
        {
          key_point += std::to_string(b) + "|";
          distance +=
              -1 * (it.second - (b / this->params["grid_bins"]));
        }
      }
    }

    // if point already exists in the array, adds an individual and the difference between them
    if(morphological_grid_generation.count(key_point)>0) {

      morphological_grid_generation[key_point].push_back(distance); // add map with key=id value=distance ?

      // if point does not exist in the array yet, , adds new point with its first individual and the difference between them
    }else {
      std::vector<double> individual; individual.push_back(distance);
      morphological_grid_generation[key_point] = individual;
    }

    // if point already exists in the array, adds an individual and the id
    if (this->morphological_grid_accumulated.count(key_point) > 0)
    {

      this->morphological_grid_accumulated[key_point].push_back
          (this->offspring[i].getId());

      // if point does not exist in the array yet, , adds new point with its first individual and the difference between them
    }
    else
    {
      std::vector< std::string > individual;
      individual.push_back(this->offspring[i].getId());
      this->morphological_grid_accumulated[key_point] = individual;
    }
  }

  // logs state of the grid
  std::ofstream myfile;
  std::string path = this->path+"experiments/" + this->experiment_name +
                     "/morphological_grid_accumulated.txt";
  myfile.open(path);
  for (const auto &it : this->morphological_grid_accumulated)
  {
    myfile << it.first + "-";
    for (int i = 0;
         i < it.second.size();
         i++)
    {
      myfile << it.second[i] << " ";
    }
    myfile << std::endl;
  }
  myfile.close();

  std::vector< int > morphological_grids;
  morphological_grids.push_back((int)morphological_grid_generation.size());
  morphological_grids.push_back((int) this->morphological_grid_accumulated.size());

  return morphological_grids;
}