/*
The MIT License

Copyright (c) 2015-2017 Albert Murienne

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include "face_filer.h"

#include "neurocl.h"

#include "imagetools/edge_detect.h"
#include "imagetools/face_detect.h"

#include <boost/lexical_cast.hpp>

#include <iostream>

using namespace neurocl;
using namespace cimg_library;

#define NEUROCL_EPOCH_SIZE 100
#define NEUROCL_BATCH_SIZE 10
#define MAX_MATCH_ERROR 0.1f

#define IMAGE_SIZEX 480
#define IMAGE_SIZEY 320

#define FACE_SIZEX 100
#define FACE_SIZEY 100

static const unsigned char green[] = { 0,255,0 };
static const unsigned char red[] = { 255,0,0 };

enum class face_type
{
    FT_GUESS = 0,
    FT_USERA,
    FT_USERB,
    FT_UNKNOWN,
    //FT_NOT_A_FACE,
    FT_MAX
};

struct face_result
{
    face_result( face_type _type, float _score1, float _score2 )
        : type( _type ), score1( _score1 ), score2( _score2 ) {}

    const std::string result()
    {
        std::string str_type;
        switch( type )
        {
        case face_type::FT_USERA:
            str_type = "YOU ARE JOHN! ";
            break;
        case face_type::FT_USERB:
            str_type = "YOU ARE JANE! ";
            break;
        case face_type::FT_UNKNOWN:
        default:
            str_type = "YOU ARE UNKNOWN... ";
            break;
        }
        return str_type
            + "(" + std::to_string( score1 ) + ";"
            + std::to_string( score2 ) + ")";
    }

    face_type type;
    float score1;
    float score2;
};

boost::optional<face_result> opt_computed_face;

void face_preprocess( CImg<>& image )
{
    CImg<float> edged_image( 50, 50, 1, 1, 0 );

    sobel::process( image, edged_image );

    //canny<float> canny( image.width(), image.height() );
    //canny.process( image, edged_image );

    edged_image.normalize( 0.f, 1.f );
    image = edged_image; // overwrite input image
    //image.display();
}

void face_preprocess_generic( float* image, const size_t sizeX, const size_t sizeY )
{
    CImg<float> _image( image, sizeX, sizeY, 1, 1, true /*shared*/ );
    face_preprocess( _image );
}

void face_process(  CImg<float> image, const face_type& ftype,
                    std::shared_ptr<network_manager_interface> net_manager,
                    iterative_trainer& trainer,
                    face_filer& face_files )
{
    image.resize( 50, 50 );
    image.equalize( 256, 0, 255 );
    image.normalize( 0.f, 1.f );
    image.channel(0);

    CImg<float> work_image = image;

    face_preprocess( work_image );

    std::string label;
    float output[2] = { 0.f, 0.f };
    sample sample( work_image.width() * work_image.height(), work_image.data(), 2, output );

    bool compute = false;

    switch( ftype )
    {
    case face_type::FT_USERA:
        label = "A";
        output[0] = 1.f;
        break;
    case face_type::FT_USERB:
        label = "B";
        output[1] = 1.f;
        break;
    case face_type::FT_UNKNOWN:
        label = "U";
        //output[2] = 1.f;
        break;
    //case FT_NOT_A_FACE:
    //    break;
    case face_type::FT_GUESS:
        compute = true;
        break;
    case face_type::FT_MAX:
    default:
        // should never be reached
    break;
    }

    if ( compute )
    {
        net_manager->compute_output( sample );

        std::cout << "max comp idx: " << sample.max_comp_idx() << " max comp val: " << sample.max_comp_val() << std::endl;

        if ( sample.max_comp_idx() == 0 )
            opt_computed_face = face_result( face_type::FT_USERA, output[0], output[1] );
        else if ( sample.max_comp_idx() == 1 )
            opt_computed_face = face_result( face_type::FT_USERB, output[0], output[1] );
    }
    else
    {
        trainer.train_new( sample );
        face_files.save_face( label, image );
    }
}

void grab_image( CImg<float>& image )
{
    int res = 0;
#ifdef __APPLE__
    // grab using ImageCapture utility
    res = system( "../../ImageCapture-v0.2/ImageCapture face_scene.png" );
#elif __arm__
    // grab using raspistill utility
    res = system( "raspistill -w 480 -h 320 -e png -o face_scene.png");
#else
    res = system( "fswebcam -r 480x320 --png -D face_scene.png");
#endif

    if ( res != -1 )
    {
    	image.load( "face_scene.png" );
    	image.resize( IMAGE_SIZEX, IMAGE_SIZEY );
    }
    else
        std::cerr << "error trying to grab webcam image!" << std::endl;
}

void draw_metadata( CImg<float>& image, const std::vector<face_detect::face_rect>& faces )
{
    std::string label( "Please center your face in the green rectangle and type:\nG = Guess?\nA = John\nE = Jane\nU = Unknown" );
    image.draw_text( 5, 5, label.c_str(), green );
    image.draw_rectangle( IMAGE_SIZEX/2 - FACE_SIZEX, IMAGE_SIZEY/2 - FACE_SIZEY,
        IMAGE_SIZEX/2 + FACE_SIZEX, IMAGE_SIZEY/2 + FACE_SIZEY, green, 1.f, ~0L );
    if ( !faces.empty() )
    {
        const face_detect::face_rect& frect = faces[0];
    	image.draw_rectangle( frect.x0, frect.y0, frect.x1, frect.y1, red, 1.f, ~0L );
    }
}

void draw_message( CImg<float>& image, const std::string& message )
{
    image.draw_text( IMAGE_SIZEX/2, IMAGE_SIZEY/2, message.c_str(), red );
}

int main ( int argc,char **argv )
{
    std::cout << "Welcome to facecam!" << std::endl;

    try
    {
        std::shared_ptr<network_manager_interface> net_manager = network_factory::build( network_factory::t_neural_impl::NEURAL_IMPL_MLP );
        net_manager->load_network( "../nets/facecam/topology-facecam.txt", "../nets/facecam/weights-facecam.bin" );

        // TODO : check command arguments with boost
        // 0 normal use
        // 1 offline training
        bool offline_training = ( argc == 2 ) && ( boost::lexical_cast<int>( argv[1] ) == 1 );

        //************************* TRAINING *************************//

        if ( offline_training )
        {
            /******** TRAIN ********/

            samples_manager smp_manager;
            smp_manager.load_samples(  "../nets/facecam/facecam-train.txt",
                                                                true /*shuffle*/,
                                                                &face_preprocess_generic /* extra_preproc*/ );

            net_manager->batch_train( smp_manager, NEUROCL_EPOCH_SIZE, NEUROCL_BATCH_SIZE );

            /******** VALIDATE ********/

            const std::vector<sample>& training_samples = smp_manager.get_samples();

            float mean_rmse = 0.f;
            size_t _rmse_score = 0;
            size_t _classif_score = 0;

            for ( size_t i = 0; i<training_samples.size(); i++ )
            {
                test_sample tsample( smp_manager.get_samples()[i] );
                net_manager->compute_output( tsample );

                std::cout << tsample.output() << std::endl;
                std::cout << tsample.ref_output() << std::endl;
                std::cout << tsample.RMSE() << std::endl;

                mean_rmse += tsample.RMSE();

                if ( tsample.RMSE() < MAX_MATCH_ERROR )
                    ++ _rmse_score;

                if ( tsample.classified() )
                    ++_classif_score;

            	//std::cout << "TEST OUTPUT IS : " << tsample.output() << std::endl;
            }

            mean_rmse /= static_cast<float>( training_samples.size() );

            std::cout << "MEAN RMSE IS " << mean_rmse << std::endl;
            std::cout << "RMSE SCORE IS " << _rmse_score << "/" << training_samples.size() << std::endl;
            std::cout << "CLASSIF SCORE IS " << _classif_score << "/" << training_samples.size() << std::endl;
        }
        else
        {
            std::vector<face_detect::face_rect> faces;
            face_detect my_face_detect;

            face_filer face_files;

            iterative_trainer trainer( net_manager, NEUROCL_BATCH_SIZE );

            CImg<float> input_image;
            CImg<float> display_image;

            CImgDisplay my_display( IMAGE_SIZEX, IMAGE_SIZEY );
            my_display.set_title( "FaceCam" );
        #ifdef __arm__
            my_display.set_fullscreen( true );
        #endif
            CImg<unsigned char> welcome( IMAGE_SIZEX, IMAGE_SIZEY, 1, 3 );
            welcome.draw_text( 50, 50, "Welcome to FaceCam\nPlease wait during capture initialization...", green );
            my_display.display( welcome );

            face_type ftype = face_type::FT_MAX;

            do
            {
                ftype = face_type::FT_MAX;

            	if ( my_display.is_key( cimg::keyG ) )
            	{
                    std::cout << "Guess that face!" << std::endl;
                    ftype = face_type::FT_GUESS;
                }
                else if ( my_display.is_key( cimg::keyA ) )
                {
                    std::cout << "This is John!" << std::endl;
                    ftype = face_type::FT_USERA;
                }
                else if ( my_display.is_key( cimg::keyE ) )
                {
                    std::cout << "This is Jane!" << std::endl;
                    ftype = face_type::FT_USERB;
                }
                else if ( my_display.is_key( cimg::keyU ) )
                {
                    std::cout << "This person is unknown!" << std::endl;
                    ftype = face_type::FT_UNKNOWN;
                }
                /*else if ( my_display.is_key( cimg::key0 ) )
                {
                    std::cout << "There is no one!" << std::endl;
                    ftype = face_type::FT_NOT_A_FACE;
                }*/
                else if ( my_display.is_key( cimg::keyQ ) || my_display.is_key( cimg::keyESC ) )
                {
                    std::cout << "Bye Bye!" << std::endl;
                    break;
                }
                else
                {
                    // UNMANAGED KEY
                    //std::cout << "unmanaged event" << std::endl;
                }

                std::cout << "key " << my_display.key() << std::endl;

                if ( ( ftype != face_type::FT_MAX ) && !faces.empty() )
                {
                    face_process(	input_image.get_crop( faces[0].x0, faces[0].y0, faces[0].x1, faces[0].y1 ), ftype,
                                    net_manager, trainer,
                                    face_files );
                }

                if ( opt_computed_face )
                {
                    std::cout << "face detected!" << std::endl;
                    draw_message( display_image, opt_computed_face.get().result() );
                }
                else
                {
                    grab_image( input_image );
                    display_image = input_image;
                    faces = my_face_detect.detect( input_image.data(), input_image.width(), input_image.height() );
                    if ( faces.empty() )
                        draw_message( display_image, "NO FACE DETECTED!" );
                	draw_metadata( display_image, faces );
                }

                my_display.display( display_image );

                do
                {
                	my_display.wait();
                }
                while( my_display.key() == 0 );

                opt_computed_face = boost::none;

            } while( !my_display.is_closed() );
        }
    }
    catch( network_exception& e )
    {
        std::cerr << "network exception : " << e.what() << std::endl;
    }
    catch( std::exception& e )
    {
        std::cerr << "std::exception : " << e.what() << std::endl;
    }
    catch(...)
    {
        std::cerr << "unknown exception" << std::endl;
    }

    std::cout << "Bye bye facecam!" << std::endl;

    return 0;
}


//
//  bbuffer.cpp
//  jpcnn
//
//  Created by Peter Warden on 1/9/14.
//  Copyright (c) 2014 Jetpac, Inc. All rights reserved.
//

#include "buffer.h"

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

#ifdef USE_ACCELERATE_GEMM
#include <Accelerate/Accelerate.h>
#endif

#ifdef USE_OS_IMAGE
#include "os_image_load.h"
#include "os_image_save.h"
#else // USE_OS_IMAGE
#include "stb_image.h"
#endif // USE_OS_IMAGE
#include "binary_format.h"
#include "cstring_helpers.h"
#ifdef TARGET_PI
#include "mailbox.h"
#endif // TARGET_PI

#ifdef USE_EIGEN_GEMM
#include <Eigen/Core>
#endif // USE_EIGEN_GEMM

static void buffer_do_save_to_image_file(Buffer* buffer, const char* filename);

Buffer::Buffer(const Dimensions& dims) :
  _dims(dims),
  _name(NULL),
  _debugString(NULL),
  _quantizedData(NULL),
  _min(0.0f),
  _max(1.0f),
  _bitsPerElement(32)
{
  const int elementCount = _dims.elementCount();
  const size_t byteCount = (elementCount * sizeof(jpfloat_t));
#if defined(TARGET_PI)
  _gpuMemoryHandle = mem_alloc(byteCount, 4096, GPU_MEM_FLG);
  if (!_gpuMemoryHandle) {
    fprintf(stderr, "Unable to allocate %d bytes of GPU memory\n", byteCount);
  }
  _gpuMemoryBase = mem_lock(_gpuMemoryHandle);
  _data = (jpfloat_t*)(mapmem(_gpuMemoryBase + GPU_MEM_MAP, byteCount));
#elif defined(USE_EIGEN_GEMM)
  _data = (jpfloat_t*)(Eigen::internal::aligned_malloc(byteCount));
#else // TARGET_PI
  _data = (jpfloat_t*)(malloc(byteCount * 1));
#endif // TARGET_PI
  _doesOwnData = true;
  setName("None");
}

Buffer::Buffer(const Dimensions& dims, jpfloat_t* data) :
  _dims(dims),
  _name(NULL),
  _debugString(NULL),
  _quantizedData(NULL),
#if defined(TARGET_PI)
  _gpuMemoryHandle(0),
  _gpuMemoryBase(0),
#endif // TARGET_PI
  _min(0.0f),
  _max(1.0f),
  _bitsPerElement(32)
{
  _data = data;
  _doesOwnData = false;
  setName("None");
}

Buffer::Buffer(const Dimensions& dims, void* quantizedData, jpfloat_t min, jpfloat_t max, int bitsPerElement) :
  _dims(dims),
  _name(NULL),
  _debugString(NULL),
  _data(NULL),
#if defined(TARGET_PI)
  _gpuMemoryHandle(0),
  _gpuMemoryBase(0),
#endif // TARGET_PI
  _min(min),
  _max(max),
  _bitsPerElement(bitsPerElement)
{
  _quantizedData = quantizedData;
  _doesOwnData = false;
  setName("None");
}

Buffer::Buffer(const Dimensions& dims, jpfloat_t min, jpfloat_t max, int bitsPerElement) :
  _dims(dims),
  _name(NULL),
  _debugString(NULL),
  _data(NULL),
  _min(min),
  _max(max),
  _bitsPerElement(bitsPerElement)
{
  const int elementCount = _dims.elementCount();
  const int sizeofElement = (_bitsPerElement / 8);
  const size_t byteCount = (elementCount * sizeofElement);

#if defined(TARGET_PI)
  _gpuMemoryHandle = mem_alloc(byteCount, 4096, GPU_MEM_FLG);
  if (!_gpuMemoryHandle) {
    fprintf(stderr, "Unable to allocate %d bytes of GPU memory\n", byteCount);
  }
  _gpuMemoryBase = mem_lock(_gpuMemoryHandle);
  _quantizedData = (char*)(mapmem(_gpuMemoryBase + GPU_MEM_MAP, byteCount));
#elif defined(USE_EIGEN_GEMM)
  _quantizedData = (void*)(Eigen::internal::aligned_malloc(byteCount));
#else // TARGET_PI
  _quantizedData = (void*)(malloc(byteCount));
#endif // TARGET_PI
  _doesOwnData = true;
  setName("None");
}

Buffer::~Buffer()
{
  if (_doesOwnData) {
#if defined(TARGET_PI)
    if (_data) {
      const int elementCount = _dims.elementCount();
      const size_t byteCount = (elementCount * sizeof(jpfloat_t));
      unmapmem(_data, byteCount);
    } else if (_quantizedData) {
      const int elementCount = _dims.elementCount();
      const int sizeofElement = (_bitsPerElement / 8);
      const size_t byteCount = (elementCount * sizeofElement);
      unmapmem(_quantizedData, byteCount);
    }
    mem_unlock(_gpuMemoryHandle);
    mem_free(_gpuMemoryHandle);
#elif defined(USE_EIGEN_GEMM)
    Eigen::internal::aligned_free(_data);
    Eigen::internal::aligned_free(_quantizedData);
#else // TARGET_PI
    free(_data);
    free(_quantizedData);
#endif // TARGET_PI
  }
  if (_debugString)
  {
    free(_debugString);
  }
  if (_name)
  {
    free(_name);
  }
}

char* Buffer::debugString() {
  if (!_debugString) {
    _debugString = (char*)(malloc(MAX_DEBUG_STRING_LEN));
  }
  snprintf(_debugString, MAX_DEBUG_STRING_LEN, "Buffer %s - %s, %d bits per element, range (%f-%f)",
    _name, _dims.debugString(), _bitsPerElement, _min, _max);
  return _debugString;
}

void Buffer::printContents(int maxElements) {
  FILE* output = stderr;
  fprintf(output, "%s : \n", debugString());
  Dimensions dims = _dims;
  const int elementCount = dims.elementCount();
  const int bitsPerElement = _bitsPerElement;
  const jpfloat_t min = _min;
  const jpfloat_t max = _max;
  const jpfloat_t range = ((max - min) / (1 << bitsPerElement));
  if (elementCount < 5000) {
    maxElements = -1;
  }
  bool isSingleImage = (dims[0] == 1);
  if (isSingleImage) {
    dims = dims.removeDimensions(1);
  }
  const int dimsLength = dims._length;
  const jpfloat_t* const data = _data;
  if (isSingleImage) {
    fprintf(output, "[");
  }
  if (dimsLength == 1) {
    const int width = dims[0];
    int xLeft;
    int xRight;
    if (maxElements < 1) {
      xLeft = -1;
      xRight = -1;
    } else {
      xLeft = (maxElements / 2);
      xRight = (width - (maxElements / 2));
    }
    fprintf(output, "[");
    for (int x = 0; x < width; x += 1) {
      if (x == xLeft) {
        fprintf(output, "...");
      }
      if ((x >= xLeft) && (x < xRight)) {
        continue;
      }
      if (x > 0) {
        fprintf(output, ", ");
      }
      jpfloat_t value;
      if (bitsPerElement == 32) {
        value = *(data + dims.offset(x));
      } else if (bitsPerElement == 16) {
        uint16_t quantizedValue = *(((uint16_t*)(_quantizedData) + dims.offset(x)));
        value = (min + (quantizedValue * range));
      } else if (bitsPerElement == 8) {
        uint8_t quantizedValue = *(((uint8_t*)(_quantizedData) + dims.offset(x)));
        value = (min + (quantizedValue * range));
      } else {
        assert(false); // Should never get here
        value = 0.0f;
      }
      fprintf(output, "%.10f", value);
    }
    fprintf(output, "]");
  } else if (dimsLength == 2) {
    const int height = dims[0];
    const int width = dims[1];

    int yTop;
    int yBottom;
    if (maxElements < 1) {
      yTop = -1;
      yBottom = -1;
    } else {
      yTop = (maxElements / 2);
      yBottom = (height - (maxElements / 2));
    }

    int xLeft;
    int xRight;
    if (maxElements < 1) {
      xLeft = -1;
      xRight = -1;
    } else {
      xLeft = (maxElements / 2);
      xRight = (width - (maxElements / 2));
    }

    fprintf(output, "[");
    for (int y = 0; y < height; y += 1) {
      if (y == yTop) {
        fprintf(output, "...\n");
      }
      if ((y >= yTop) && (y < yBottom)) {
        continue;
      }
      fprintf(output, "[");
      for (int x = 0; x < width; x += 1) {
        if (x == xLeft) {
          fprintf(output, "...");
        }
        if ((x >= xLeft) && (x < xRight)) {
          continue;
        }
        if (x > 0) {
          fprintf(output, ", ");
        }
        jpfloat_t value;
        if (bitsPerElement == 32) {
          value = *(data + dims.offset(y, x));
        } else if (bitsPerElement == 16) {
          uint16_t quantizedValue = *(((uint16_t*)(_quantizedData) + dims.offset(y, x)));
          value = (min + (quantizedValue * range));
        } else if (bitsPerElement == 8) {
          uint8_t quantizedValue = *(((uint8_t*)(_quantizedData) + dims.offset(y, x)));
          value = (min + (quantizedValue * range));
        } else {
          assert(false); // Should never get here
          value = 0.0f;
        }
        fprintf(output, "%.10f", value);
      }
      if (y < (height - 1)) {
        fprintf(output, "],\n");
      } else {
        fprintf(output, "]");
      }
    }
    fprintf(output, "]");
  } else if (dimsLength == 3) {
    const int height = dims[0];
    const int width = dims[1];
    const int channels = dims[2];

    int yTop;
    int yBottom;
    if (maxElements < 1) {
      yTop = -1;
      yBottom = -1;
    } else {
      yTop = (maxElements / 2);
      yBottom = (height - (maxElements / 2));
    }

    int xLeft;
    int xRight;
    if (maxElements < 1) {
      xLeft = -1;
      xRight = -1;
    } else {
      xLeft = (maxElements / 2);
      xRight = (width - (maxElements / 2));
    }

    fprintf(output, "[");
    for (int y = 0; y < height; y += 1) {
      if (y == yTop) {
        fprintf(output, "...\n");
      }
      if ((y >= yTop) && (y < yBottom)) {
        continue;
      }
      fprintf(output, "[");
      for (int x = 0; x < width; x += 1) {
        if (x == xLeft) {
          fprintf(output, "...");
        }
        if ((x >= xLeft) && (x < xRight)) {
          continue;
        }
        if (x > 0) {
          fprintf(output, ", ");
        }
        fprintf(output, "(");
        for (int channel = 0; channel < channels; channel += 1) {
          jpfloat_t value;
          if (bitsPerElement == 32) {
            value = *(data + dims.offset(y, x, channel));
          } else if (bitsPerElement == 16) {
            uint16_t quantizedValue = *(((uint16_t*)(_quantizedData) + dims.offset(y, x, channel)));
            value = (min + (quantizedValue * range));
          } else if (bitsPerElement == 8) {
            uint8_t quantizedValue = *(((uint8_t*)(_quantizedData) + dims.offset(y, x, channel)));
            value = (min + (quantizedValue * range));
          } else {
            assert(false); // Should never get here
            value = 0.0f;
          }
          if (channel > 0) {
            fprintf(output, ", ");
          }
          fprintf(output, "%.10f", value);
        }
        fprintf(output, ")");
      }
      if (y < (height - 1)) {
        fprintf(output, "],\n");
      } else {
        fprintf(output, "]");
      }
    }
    fprintf(output, "]");
  } else {
    fprintf(output, "Printing of buffers with %d dimensions is not supported\n", dimsLength);
  }
  if (isSingleImage) {
    fprintf(output, "]");
  }
  fprintf(output, "\n");
}

void Buffer::setName(const char* name) {
  if (_name)
  {
    free(_name);
  }
  size_t byteCount = (strlen(name) + 1);
  _name = (char*)(malloc(byteCount));
  strncpy(_name, name, byteCount);
}

void Buffer::saveDebugImage() {
  buffer_save_to_image_file(this, this->_name);
}

bool Buffer::canReshapeTo(const Dimensions& newDims) {
  const int oldElementCount = _dims.elementCount();
  const int newElementCount = newDims.elementCount();
  const bool canReshape = (oldElementCount == newElementCount);
  return canReshape;
}

void Buffer::reshape(const Dimensions& newDims) {
  assert(canReshapeTo(newDims));
  _dims = newDims;
}

Buffer* Buffer::view() {
  Buffer* result = new Buffer(_dims, _data);
  char copyName[MAX_DEBUG_STRING_LEN];
  snprintf(copyName, MAX_DEBUG_STRING_LEN, "%s (view)", _name);
  result->setName(copyName);
#if defined(TARGET_PI)
  result->_gpuMemoryHandle = _gpuMemoryHandle;
  result->_gpuMemoryBase = _gpuMemoryBase;
#endif // TARGET_PI
  return result;
}

void Buffer::copyDataFrom(const Buffer* other) {
  const Dimensions& myDims = _dims;
  const Dimensions& otherDims = other->_dims;
  const size_t myElementCount = myDims.elementCount();
  const size_t otherElementCount = otherDims.elementCount();
  assert(myElementCount == otherElementCount);
  jpfloat_t* myData = _data;
  const jpfloat_t* otherData = other->_data;
  const size_t myByteCount = (myElementCount * sizeof(jpfloat_t));
  memcpy(myData, otherData, myByteCount);
}

void Buffer::convertFromChannelMajor(const Dimensions& expectedDims) {
  if (expectedDims._length < 4) {
    // No channels in the lower dimensional images, so skip any conversion
    return;
  }
  assert(canReshapeTo(expectedDims));
  const int imageCount = expectedDims[0];
  const int height = expectedDims[1];
  const int width = expectedDims[2];
  const int channels = expectedDims[3];

  Dimensions oldDims(imageCount, channels, height, width);

  const int elementCount = expectedDims.elementCount();
  const size_t byteCount = (elementCount * sizeof(jpfloat_t));
  jpfloat_t* newData = (jpfloat_t*)(malloc(byteCount));
  jpfloat_t* oldData = _data;

  for (int imageIndex = 0; imageIndex < imageCount; imageIndex += 1) {
    for (int y = 0; y < height; y += 1) {
      for (int x = 0; x < width; x += 1) {
        for (int channel = 0; channel < channels; channel += 1) {
          jpfloat_t* source = (oldData + oldDims.offset(imageIndex, channel, y, x));
          jpfloat_t* dest = (newData + expectedDims.offset(imageIndex, y, x, channel));
          *dest = *source;
        }
      }
    }
  }

  if (_doesOwnData) {
    free(_data);
  }

  _data = newData;
  _doesOwnData = true;
}

void Buffer::populateWithRandomValues(jpfloat_t min, jpfloat_t max) {
  const int elementCount = _dims.elementCount();
  const int bitsPerElement = _bitsPerElement;
  if (bitsPerElement == 32) {
    jpfloat_t* dataStart = _data;
    jpfloat_t* dataEnd = (_data + elementCount);
    jpfloat_t* data = dataStart;
    while (data < dataEnd) {
      // rand() is awful, but as long as we're using this for debugging
      // it should be good enough, and avoids dependencies.
      *data = (((max - min) * ((float)rand() / RAND_MAX)) + min);
      data += 1;
    }
  } else if (bitsPerElement == 16) {
    uint16_t* dataStart = (uint16_t*)(_quantizedData);
    uint16_t* dataEnd = (dataStart + elementCount);
    uint16_t* data = dataStart;
    const int integerMin = (((min - _min) * (1 << bitsPerElement)) / (_max - _min));
    const int integerMax = (((max - _min) * (1 << bitsPerElement)) / (_max - _min));
    while (data < dataEnd) {
      // rand() is awful, but as long as we're using this for debugging
      // it should be good enough, and avoids dependencies.
      *data = (((integerMax - integerMin) * ((float)rand() / RAND_MAX)) + integerMin);
      data += 1;
    }
  } else if (bitsPerElement == 8) {
    uint8_t* dataStart = (uint8_t*)(_quantizedData);
    uint8_t* dataEnd = (dataStart + elementCount);
    uint8_t* data = dataStart;
    const int integerMin = (((min - _min) * (1 << bitsPerElement)) / (_max - _min));
    const int integerMax = (((max - _min) * (1 << bitsPerElement)) / (_max - _min));
    while (data < dataEnd) {
      // rand() is awful, but as long as we're using this for debugging
      // it should be good enough, and avoids dependencies.
      *data = (((integerMax - integerMin) * ((float)rand() / RAND_MAX)) + integerMin);
      data += 1;
    }
  } else {
    assert(false); // Should never get here
  }
}

void Buffer::quantize(int bits) {
  jpfloat_t min = FLT_MAX;
  jpfloat_t max = -FLT_MAX;

  const int elementCount = _dims.elementCount();
  jpfloat_t* dataStart = _data;
  jpfloat_t* dataEnd = (_data + elementCount);
  jpfloat_t* data = dataStart;
  while (data < dataEnd) {
    const jpfloat_t value = *data;
    min = fminf(min, value);
    max = fmaxf(max, value);
    data += 1;
  }

  const int levels = (1 << bits);

  const jpfloat_t spread = ((max - min) / levels);
  const jpfloat_t recipSpread = (1.0f / fmaxf(0.00000001f, spread));

  fprintf(stderr, "min = %f, max = %f\n", min, max);

  data = dataStart;
  while (data < dataEnd) {
    const jpfloat_t value = *data;
    const int quantized = (int)roundf((value - min) * recipSpread);
    const jpfloat_t dequantized = ((quantized * spread) + min);
    *data = dequantized;
    data += 1;
  }

}

void Buffer::transpose() {
  const Dimensions& originalDims = _dims;
  assert(originalDims._length == 2); // expecting width x height
  const int originalHeight = originalDims[0];
  const int originalWidth = originalDims[1];
  const int bitsPerElement = _bitsPerElement;
  const int bytesPerElement = (bitsPerElement / 8);

  const int newHeight = originalWidth;
  const int newWidth = originalHeight;

  Dimensions newDims(newHeight, newWidth);

  const int elementCount = originalDims.elementCount();
  const size_t byteCount = (elementCount * bytesPerElement);
  void* newDataBytes = malloc(byteCount);

  if (bitsPerElement == 32) {
    jpfloat_t* originalData = _data;
    jpfloat_t* newData = (jpfloat_t*)(newDataBytes);
    for (int originalY = 0; originalY < originalHeight; originalY += 1) {
      for (int originalX = 0; originalX < originalWidth; originalX += 1) {
        jpfloat_t* source = (originalData + originalDims.offset(originalY, originalX));
        jpfloat_t* dest = (newData + newDims.offset(originalX, originalY));
        *dest = *source;
      }
    }
    _data = newData;
  } else if (bitsPerElement == 16) {
    uint16_t* originalData = (uint16_t*)(_quantizedData);
    uint16_t* newData = (uint16_t*)(newDataBytes);
    for (int originalY = 0; originalY < originalHeight; originalY += 1) {
      for (int originalX = 0; originalX < originalWidth; originalX += 1) {
        uint16_t* source = (originalData + originalDims.offset(originalY, originalX));
        uint16_t* dest = (newData + newDims.offset(originalX, originalY));
        *dest = *source;
      }
    }
    _quantizedData = newData;
  } else if (bitsPerElement == 8) {
    uint8_t* originalData = (uint8_t*)(_quantizedData);
    uint8_t* newData = (uint8_t*)(newDataBytes);
    for (int originalY = 0; originalY < originalHeight; originalY += 1) {
      for (int originalX = 0; originalX < originalWidth; originalX += 1) {
        uint8_t* source = (originalData + originalDims.offset(originalY, originalX));
        uint8_t* dest = (newData + newDims.offset(originalX, originalY));
        *dest = *source;
      }
    }
    _quantizedData = newData;
  } else {
    assert(false); // should never get here
  }

  if (_doesOwnData) {
    free(_data);
  }

  _dims = newDims;
  _doesOwnData = true;
}

Buffer* buffer_from_image_file(const char* filename)
{
  int inputWidth;
  int inputHeight;
  int inputChannels;

  const bool isRaw = string_ends_with(filename, ".raw");

  uint8_t* imageData;
  if (isRaw) {
    FILE* inputFile = fopen(filename, "rb");
    if (!inputFile) {
      fprintf(stderr, "jpcnn couldn't open '%s'\n", filename);
      return NULL;
    }
    fseek(inputFile, 0, 2);
    const size_t bytesInFile = ftell(inputFile);
    fseek(inputFile, 0, 0);
    const int imageSize = 256;
    const size_t bytesPerImage = (imageSize * imageSize * 3);
    const size_t bytesPerImagePlusLabel = (bytesPerImage + 4);
    const size_t imageCount = (bytesInFile / bytesPerImagePlusLabel);
    if ((imageCount * bytesPerImagePlusLabel) != bytesInFile) {
      fprintf(stderr, "Bad file size %zu for %s - expected %zux%dx%dx3 + %zux4\n",
        bytesInFile, filename, imageCount, imageSize, imageSize, imageCount);
      return NULL;
    }
    uint8_t* fileData = (uint8_t*)(malloc(bytesInFile));
    fread(fileData, bytesInFile, 1, inputFile);
    fclose(inputFile);

    uint8_t* channeledImageData = (fileData + (imageCount * 4));
    const size_t bytesPerChannel = (imageSize * imageSize);
    uint8_t* inputRed = (channeledImageData + (0 * bytesPerChannel));
    uint8_t* inputGreen = (channeledImageData + (1 * bytesPerChannel));
    uint8_t* inputBlue = (channeledImageData + (2 * bytesPerChannel));

    const int outputChannels = 4;
    const size_t bytesPerOutputImage = (bytesPerChannel * outputChannels);

    uint8_t* outputImageData = (uint8_t*)(malloc(bytesPerOutputImage));
    uint8_t* output = outputImageData;
    uint8_t* outputEnd = (outputImageData + bytesPerOutputImage);
    while (output < outputEnd) {
      *output = *inputRed;
      inputRed += 1;
      output += 1;
      *output = *inputGreen;
      inputGreen += 1;
      output += 1;
      *output = *inputBlue;
      inputBlue += 1;
      output += 1;
      if (outputChannels > 3) {
        *output = 255;
        output += 1;
      }
    }
    free(fileData);

    imageData = outputImageData;
    inputWidth = imageSize;
    inputHeight = imageSize;
    inputChannels = outputChannels;

  } else {
#ifdef USE_OS_IMAGE
    imageData = os_image_load_from_file(filename, &inputWidth, &inputHeight, &inputChannels, 0);
#else // USE_OS_IMAGE
    FILE* inputFile = fopen(filename, "rb");
    if (!inputFile) {
      fprintf(stderr, "jpcnn couldn't open '%s'\n", filename);
      return NULL;
    }
    imageData = stbi_load_from_file(inputFile, &inputWidth, &inputHeight, &inputChannels, 0);
    fclose(inputFile);
#endif // USE_OS_IMAGE
  }
  if (!imageData) {
    fprintf(stderr, "jpcnn couldn't read '%s'\n", filename);
    return NULL;
  }

  Dimensions dims(inputHeight, inputWidth, inputChannels);
  Buffer* buffer = new Buffer(dims);
  buffer->setName(filename);

  jpfloat_t* bufferCurrent = buffer->_data;
  jpfloat_t* bufferEnd = (buffer->_data + buffer->_dims.elementCount());
  uint8_t* imageCurrent = imageData;
  while (bufferCurrent != bufferEnd)
  {
    *bufferCurrent = *imageCurrent;
    bufferCurrent += 1;
    imageCurrent += 1;
  }

  if (isRaw) {
    free(imageData);
  } else {
#ifdef USE_OS_IMAGE
    os_image_free(imageData);
#else // USE_OS_IMAGE
    stbi_image_free(imageData);
#endif // USE_OS_IMAGE
  }

  return buffer;
}

Buffer* buffer_from_dump_file(const char* filename)
{
  SBinaryTag* mainDict = read_tag_from_file(filename, false);
  assert(mainDict != NULL);

  Buffer* result = buffer_from_tag_dict(mainDict, false);
  result->setName(filename);

  deallocate_file_tag(mainDict, false);

  return result;
}

Buffer* buffer_from_tag_dict(SBinaryTag* mainDict, bool skipCopy) {

  SBinaryTag* bitsPerFloatTag = get_tag_from_dict(mainDict, "float_bits");
  const uint32_t bitsPerFloat = bitsPerFloatTag->payload.jpuint;
  if ((bitsPerFloat != 32) && (bitsPerFloat != 16) && (bitsPerFloat != 8)) {
    fprintf(stderr, "jpcnn can only read 32, 16 or 8 bit dump files, found %d\n", bitsPerFloat);
    return NULL;
  }

  SBinaryTag* dimsTag = get_tag_from_dict(mainDict, "dims");
  assert(dimsTag->type == JP_LIST);
  int32_t dimensions[DIMENSIONS_MAX_LENGTH];
  int32_t dimensionsCount = 0;
  char* current = dimsTag->payload.jpchar;
  char* end = (current + dimsTag->length);
  while (current < end) {
    SBinaryTag* entryTag = get_tag_from_memory(current, end);
    current += get_total_sizeof_tag(entryTag);
    assert(entryTag->type == JP_UINT);
    dimensions[dimensionsCount] = entryTag->payload.jpuint;
    dimensionsCount += 1;
  }

  Dimensions dims(dimensions, dimensionsCount);
  const int elementCount = dims.elementCount();

  Buffer* buffer;
  if (bitsPerFloat == 32) {
    SBinaryTag* dataTag = get_tag_from_dict(mainDict, "data");
    assert(dataTag->type == JP_FARY);
    assert(dataTag->length == (elementCount * sizeof(jpfloat_t)));
    if (skipCopy) {
      jpfloat_t* tagDataArray = dataTag->payload.jpfary;
      buffer = new Buffer(dims, tagDataArray);
    } else {
      buffer = new Buffer(dims);
      memcpy(buffer->_data, dataTag->payload.jpchar, dataTag->length);
    }
  } else {
    SBinaryTag* quantizedDataTag = get_tag_from_dict(mainDict, "quantized_data");
    const size_t sizeofElement = (bitsPerFloat / 8);
    assert(quantizedDataTag->type == JP_BLOB);
    assert(quantizedDataTag->length == (elementCount * sizeofElement));
    jpfloat_t min = get_float_from_dict(mainDict, "min");
    jpfloat_t max = get_float_from_dict(mainDict, "max");
    void* tagDataArray = quantizedDataTag->payload.jpchar;
    if (skipCopy) {
      buffer = new Buffer(dims, tagDataArray, min, max, bitsPerFloat);
    } else {
#ifdef LOAD_BUFFERS_AS_FLOAT
      buffer = new Buffer(dims);
      jpfloat_t range = ((max - min) / (1 << bitsPerFloat));
      const size_t elementsCount = dims.elementCount();
      if (bitsPerFloat == 16) {
        uint16_t* quantizedData = (uint16_t*)tagDataArray;
        jpfloat_t* floatData = buffer->_data;
#ifdef USE_ACCELERATE_GEMM
        vDSP_vfltu16(
          quantizedData,
          1,
          floatData,
          1,
          elementsCount);
        vDSP_vsmsa(
          floatData,
          1,
          &range,
          &min,
          floatData,
          1,
          elementsCount
        );
#else // USE_ACCELERATE_GEMM
        jpfloat_t* current = floatData;
        jpfloat_t* end = (current + elementsCount);
        uint16_t* quantized = quantizedData;
        while (current < end) {
          *current = (min + ((*quantized) * range));
          current += 1;
          quantized += 1;
        }
#endif // USE_ACCELERATE_GEMM
    } else if (bitsPerFloat == 8) {
        uint8_t* quantizedData = (uint8_t*)tagDataArray;
        jpfloat_t* floatData = buffer->_data;
#ifdef USE_ACCELERATE_GEMM
        vDSP_vfltu8(
          quantizedData,
          1,
          floatData,
          1,
          elementsCount);
        vDSP_vsmsa(
          floatData,
          1,
          &range,
          &min,
          floatData,
          1,
          elementsCount
        );
#else // USE_ACCELERATE_GEMM
        jpfloat_t* current = floatData;
        jpfloat_t* end = (current + elementsCount);
        uint8_t* quantized = quantizedData;
        while (current < end) {
          *current = (min + ((*quantized) * range));
          current += 1;
          quantized += 1;
        }
#endif // USE_ACCELERATE_GEMM
      } else {
        assert(false); // Should never get here, only 8 or 16 bit supported
      }
#else // LOAD_BUFFERS_AS_FLOAT
      buffer = new Buffer(dims, min, max, bitsPerFloat);
      memcpy(buffer->_quantizedData, tagDataArray, quantizedDataTag->length);
#endif // LOAD_BUFFERS_AS_FLOAT
    }
  }

  return buffer;
}

SBinaryTag* buffer_to_tag_dict(Buffer* buffer, int floatBits) {
  SBinaryTag* mainDict = create_dict_tag();
  mainDict = add_uint_to_dict(mainDict, "float_bits", floatBits);

  assert((floatBits == 32) || (floatBits == 16) || (floatBits == 8));

  SBinaryTag* dimsTag = create_list_tag();
  for (int index = 0; index < buffer->_dims._length; index += 1) {
    dimsTag = add_uint_to_list(dimsTag, buffer->_dims[index]);
  }
  mainDict = add_tag_to_dict(mainDict, "dims", dimsTag);
  free(dimsTag);

  if ((floatBits == 8) || (floatBits == 16)) {
    jpfloat_t min;
    jpfloat_t max;
    void* quantizedData;
    size_t sizeofQuantizedData;
    if (buffer->_bitsPerElement == 32) {
      quantize_buffer(buffer, floatBits, &min, &max, &quantizedData, &sizeofQuantizedData);
    } else {
      assert(floatBits == buffer->_bitsPerElement);
      quantizedData = buffer->_quantizedData;
      min = buffer->_min;
      max = buffer->_max;
      const int bytesPerElement = (buffer->_bitsPerElement / 8);
      const int elementCount = buffer->_dims.elementCount();
      sizeofQuantizedData = (bytesPerElement * elementCount);
    }

    mainDict = add_float_to_dict(mainDict, "min", min);
    mainDict = add_float_to_dict(mainDict, "max", max);
    mainDict = add_blob_to_dict(mainDict, "quantized_data", quantizedData, (int)sizeofQuantizedData);

    if (buffer->_bitsPerElement == 32) {
      free(quantizedData);
    }
  } else if (floatBits == 32) {
    mainDict = add_float_array_to_dict(mainDict, "data", buffer->_data, buffer->_dims.elementCount());
  }

  return mainDict;
}

void buffer_dump_to_file(Buffer* buffer, const char* filename) {
  SBinaryTag* mainDict = buffer_to_tag_dict(buffer);
  FILE* outputFile = fopen(filename, "wb");
  assert(outputFile != NULL);
  fwrite(mainDict, (mainDict->length + 8), 1, outputFile);
  fclose(outputFile);
  free(mainDict);
}

void buffer_save_to_image_file(Buffer* buffer, const char* basename) {
  const int maxFilenameLength = 1024;
  char filename[maxFilenameLength];

  const Dimensions dims = buffer->_dims;
  assert((dims._length == 4) || (dims._length == 3));

#ifdef USE_OS_IMAGE
  const char* imageSuffix = "png";
#else // USE_OS_IMAGE
  const char* imageSuffix = "ppm";
#endif // USE_OS_IMAGE

  if (dims._length == 3) {
    snprintf(filename, maxFilenameLength, "%s.%s", basename, imageSuffix);
    buffer_do_save_to_image_file(buffer, filename);
  } else {
    for (int index = 0; index < dims[0]; index += 1) {
      Buffer* view = buffer_view_at_top_index(buffer, index);
      snprintf(filename, maxFilenameLength, "%s_%02d.%s", basename, index, imageSuffix);
      buffer_do_save_to_image_file(view, filename);
    }
  }
}

void buffer_do_save_to_image_file(Buffer* buffer, const char* filename) {

  const Dimensions dims = buffer->_dims;
  assert(dims._length == 3);

  const int width = dims[1];
  const int height = dims[0];
  const int channels = MIN(dims[2], 3);

  const jpfloat_t* const data = buffer->_data;

#ifdef USE_OS_IMAGE

  const size_t bytesPerRow = (width * channels);
  const size_t bytesPerImage = (height * bytesPerRow);
  uint8_t* pixelData = (uint8_t*)(malloc(bytesPerImage));
  for (int y = 0; y < height; y += 1) {
    uint8_t* rowData = (pixelData + (y * bytesPerRow));
    for (int x = 0; x < width; x += 1) {
      uint8_t* pixelData = (rowData + (x * channels));
      for (int channel = 0; channel < 3; channel += 1) {
        int value;
        if (channel >= channels) {
          value = 0;
        } else {
          value = (int)(*(data + dims.offset(y, x, channel)));
        }
        pixelData[channel] = value;
      }
    }
  }

  os_image_save_to_file(filename, pixelData, width, height, channels);

#else // USE_OS_IMAGE

  FILE* outputFile = fopen(filename, "w");
  if (!outputFile) {
    fprintf(stderr, "jpcnn couldn't open '%s' for writing\n", filename);
    return;
  }

  fprintf(outputFile, "P3\n");
  fprintf(outputFile, "# CREATOR: jpcnn from %s\n", buffer->debugString());

  fprintf(outputFile, "%d %d\n", width, height);
  fprintf(outputFile, "255\n");

  for (int y = 0; y < height; y += 1) {
    for (int x = 0; x < width; x += 1) {
      for (int channel = 0; channel < 3; channel += 1) {
        int value;
        if (channel >= channels) {
          value = 0;
        } else {
          value = (int)(*(data + dims.offset(y, x, channel)));
        }
        fprintf(outputFile, "%d ", value);
      }
    }
  }
  fclose(outputFile);

#endif // USE_OS_IMAGE
}

bool buffer_are_all_close(Buffer* a, Buffer* b, jpfloat_t tolerance) {

  if (a == NULL) {
    fprintf(stderr, "Buffer a is NULL\n");
    return false;
  }

  if (b == NULL) {
    fprintf(stderr, "Buffer b is NULL\n");
    return false;
  }

  if (a->_dims._length != b->_dims._length) {
    fprintf(stderr, "Buffers have different numbers of dimensions - %s vs %s\n", a->debugString(), b->debugString());
    return false;
  }

  for (int index = 0; index < a->_dims._length; index += 1) {
    if (a->_dims._dims[index] != b->_dims._dims[index]) {
      fprintf(stderr, "Buffers are different sizes - %s vs %s\n", a->debugString(), b->debugString());
      return false;
    }
  }

  int differentCount = 0;
  jpfloat_t totalDelta = 0.0f;
  jpfloat_t* aCurrent = a->_data;
  jpfloat_t* aEnd = (a->_data + a->_dims.elementCount());
  jpfloat_t* bCurrent = b->_data;
  while (aCurrent != aEnd) {
    const jpfloat_t aValue = *aCurrent;
    const jpfloat_t bValue = *bCurrent;
    const jpfloat_t delta = (aValue - bValue);
    const jpfloat_t absDelta = fabsf(delta);
    if (absDelta > tolerance) {
      differentCount += 1;
    }
    totalDelta += absDelta;
    aCurrent += 1;
    bCurrent += 1;
  }

  if (differentCount > 0) {
    const jpfloat_t differentPercentage = 100 * (differentCount / (jpfloat_t)(a->_dims.elementCount()));
    const jpfloat_t meanDelta = (totalDelta / a->_dims.elementCount());
    fprintf(stderr, "Buffers contained %f%% different values (%d), mean delta = %f - %s vs %s\n",
      differentPercentage,
      differentCount,
      meanDelta,
      a->debugString(),
      b->debugString());
    return false;
  }

  return true;
}

Buffer* buffer_view_at_top_index(Buffer* input, int index) {
  const Dimensions inputDims = input->_dims;
  assert(inputDims._length > 1);
  assert(index < inputDims[0]);
  Dimensions outputDims = inputDims.removeDimensions(1);
  const int topStride = outputDims.elementCount();
  jpfloat_t* const viewData = (input->_data + (topStride * index));
  Buffer* output = new Buffer(outputDims, viewData);
#if defined(TARGET_PI)
  output->_gpuMemoryHandle = input->_gpuMemoryHandle;
  output->_gpuMemoryBase = input->_gpuMemoryBase;
#endif // TARGET_PI
  return output;
}

Buffer* convert_from_channeled_rgb_image(Buffer* input) {
  const Dimensions dims = input->_dims;
  assert(dims._length == 3);
  const int width = dims[1];
  const int height = dims[0];
  const int channels = dims[2];
  assert(channels == 3);

  Buffer* result = new Buffer(dims);

  jpfloat_t* inputData = input->_data;
  jpfloat_t* outputData = result->_data;

  const size_t bytesPerChannel = (width * height);
  const size_t bytesPerImage = (bytesPerChannel * channels);
  jpfloat_t* inputRed = (inputData + (0 * bytesPerChannel));
  jpfloat_t* inputGreen = (inputData + (1 * bytesPerChannel));
  jpfloat_t* inputBlue = (inputData + (2 * bytesPerChannel));

  jpfloat_t* output = outputData;
  jpfloat_t* outputEnd = (outputData + bytesPerImage);
  while (output < outputEnd) {
    *output = *inputRed;
    inputRed += 1;
    output += 1;
    *output = *inputGreen;
    inputGreen += 1;
    output += 1;
    *output = *inputBlue;
    inputBlue += 1;
    output += 1;
  }

  return result;
}

Buffer* convert_to_channeled_rgb_image(Buffer* input) {
  const Dimensions dims = input->_dims;
  assert(dims._length == 3);
  const int width = dims[1];
  const int height = dims[0];
  const int channels = dims[2];
  assert(channels == 3);

  Buffer* result = new Buffer(dims);

  jpfloat_t* inputData = input->_data;
  jpfloat_t* outputData = result->_data;

  const size_t bytesPerChannel = (width * height);
  const size_t bytesPerImage = (bytesPerChannel * channels);
  jpfloat_t* outputRed = (outputData + (0 * bytesPerChannel));
  jpfloat_t* outputGreen = (outputData + (1 * bytesPerChannel));
  jpfloat_t* outputBlue = (outputData + (2 * bytesPerChannel));

  jpfloat_t* currentInput = inputData;
  jpfloat_t* inputEnd = (inputData + bytesPerImage);
  while (currentInput < inputEnd) {
    *outputRed = *currentInput;
    outputRed += 1;
    currentInput += 1;
    *outputGreen = *currentInput;
    outputGreen += 1;
    currentInput += 1;
    *outputBlue = *currentInput;
    outputBlue += 1;
    currentInput += 1;
  }

  return result;
}

Buffer* extract_subregion(Buffer* input, const Offset& origin, const Dimensions& size) {
  const Dimensions& inputDims = input->_dims;
  assert(origin._length == size._length);
  assert(origin._length == 3); // We only handle this case right now
  assert(origin[2] == 0);
  assert(size[2] == inputDims[2]);

  const int inputWidth = inputDims[1];
  const int inputChannels = inputDims[2];

  const int regionWidth = size[1];
  const int regionChannels = size[2];

  const int bitsPerElement = (input->_bitsPerElement);
  const int bytesPerElement = (bitsPerElement / 8);

  Buffer* output;
  if (bitsPerElement == 32) {
    output = new Buffer(size);
  } else {
    output = new Buffer(size, input->_min, input->_max, bitsPerElement);
  }

  const size_t elementsPerInputRow = (inputWidth * inputChannels);
  const size_t bytesPerInputRow = (elementsPerInputRow * bytesPerElement);
  const size_t elementsPerRegionRow = (regionWidth * regionChannels);
  const size_t bytesPerRegionRow = (elementsPerRegionRow * bytesPerElement);

  const size_t inputOffset = inputDims.offset(origin[0], origin[1], origin[2]);
  const uint8_t* inputByteDataStart;
  if (bitsPerElement == 32) {
    inputByteDataStart = (uint8_t*)(input->_data);
  } else {
    inputByteDataStart = (uint8_t*)(input->_quantizedData);
  }
  const uint8_t* inputByteData = (inputByteDataStart + (inputOffset * bytesPerElement));
  uint8_t* regionByteData;
  if (bitsPerElement == 32) {
    regionByteData = (uint8_t*)(output->_data);
  } else {
    regionByteData = (uint8_t*)(output->_quantizedData);
  }
  uint8_t* const regionByteEnd = (regionByteData + (size.elementCount() * bytesPerElement));
  while (regionByteData < regionByteEnd) {
    memcpy(regionByteData, inputByteData, bytesPerRegionRow);
    inputByteData += bytesPerInputRow;
    regionByteData += bytesPerRegionRow;
  }

  return output;
}

void quantize_buffer(Buffer* input, int howManyBits, jpfloat_t* outMin, jpfloat_t* outMax, void** outData, size_t* outSizeofData) {
  assert((howManyBits == 8) || (howManyBits == 16));

  jpfloat_t min = FLT_MAX;
  jpfloat_t max = -FLT_MAX;

  const int elementCount = input->_dims.elementCount();
  jpfloat_t* dataStart = input->_data;
  jpfloat_t* dataEnd = (input->_data + elementCount);
  jpfloat_t* data = dataStart;
  while (data < dataEnd) {
    const jpfloat_t value = *data;
    min = fminf(min, value);
    max = fmaxf(max, value);
    data += 1;
  }

  const int levels = (1 << howManyBits);

  const jpfloat_t spread = ((max - min) / levels);
  const jpfloat_t recipSpread = (1.0f / fmaxf(0.00000001f, spread));

  if (howManyBits == 8) {
    const size_t sizeofQuantizedData = elementCount * sizeof(uint8_t);
    uint8_t* quantizedDataStart = (uint8_t*)(malloc(sizeofQuantizedData));
    data = dataStart;
    uint8_t* quantizedData = quantizedDataStart;
    while (data < dataEnd) {
      const jpfloat_t value = *data;
      int quantized = (int)roundf((value - min) * recipSpread);
      if (quantized < 0) {
        quantized = 0;
      } else if (quantized >= levels) {
        quantized = (levels - 1);
      }
      *quantizedData = (uint8_t)(quantized);
      data += 1;
      quantizedData += 1;
    }
    *outData = quantizedDataStart;
    *outSizeofData = sizeofQuantizedData;
  } else if (howManyBits == 16) {
    const size_t sizeofQuantizedData = elementCount * sizeof(uint16_t);
    uint16_t* quantizedDataStart = (uint16_t*)(malloc(sizeofQuantizedData));
    data = dataStart;
    uint16_t* quantizedData = quantizedDataStart;
    while (data < dataEnd) {
      const jpfloat_t value = *data;
      int quantized = (int)roundf((value - min) * recipSpread);
      if (quantized < 0) {
        quantized = 0;
      } else if (quantized >= levels) {
        quantized = (levels - 1);
      }
      *quantizedData = (uint16_t)(quantized);
      data += 1;
      quantizedData += 1;
    }
    *outData = quantizedDataStart;
    *outSizeofData = sizeofQuantizedData;
  } else {
    assert(false); // should never get here
    *outData = NULL;
    *outSizeofData = 0;
  }
  *outMin = min;
  *outMax = max;
}


// Copyright 2020 Tier IV, Inc.
//
// 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 "obstacle_stop_planner/adaptive_cruise_control.hpp"

#include <boost/algorithm/clamp.hpp>
#include <boost/assert.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/format.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include <algorithm>
#include <limits>
#include <string>
#include <vector>

namespace bg = boost::geometry;
using Point = bg::model::d2::point_xy<double>;
using Polygon = bg::model::polygon<Point, false>;
using Line = bg::model::linestring<Point>;

namespace
{
Point convertPointRosToBoost(const geometry_msgs::msg::Point & point)
{
  const Point point2d(point.x, point.y);
  return point2d;
}

geometry_msgs::msg::Vector3 rpyFromQuat(const geometry_msgs::msg::Quaternion & q)
{
  tf2::Quaternion quat(q.x, q.y, q.z, q.w);
  tf2::Matrix3x3 mat(quat);
  double roll, pitch, yaw;
  mat.getRPY(roll, pitch, yaw);
  geometry_msgs::msg::Vector3 rpy;
  rpy.x = roll;
  rpy.y = pitch;
  rpy.z = yaw;
  return rpy;
}

Polygon getPolygon(
  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size,
  const double center_offset, const double l_margin = 0.0, const double w_margin = 0.0)
{
  Polygon obj_poly;
  geometry_msgs::msg::Vector3 obj_rpy = rpyFromQuat(pose.orientation);

  double l = size.x * std::cos(obj_rpy.y) + l_margin;
  double w = size.y * std::cos(obj_rpy.x) + w_margin;
  double co = center_offset;
  bg::exterior_ring(obj_poly) = boost::assign::list_of<Point>(l / 2.0 + co, w / 2.0)(
    -l / 2.0 + co, w / 2.0)(-l / 2.0 + co, -w / 2.0)(l / 2.0 + co, -w / 2.0)(l / 2.0 + co, w / 2.0);

  // rotate polygon
  bg::strategy::transform::rotate_transformer<bg::radian, double, 2, 2> rotate(
    -obj_rpy.z);  // original:clockwise rotation
  Polygon rotate_obj_poly;
  bg::transform(obj_poly, rotate_obj_poly, rotate);
  // translate polygon
  bg::strategy::transform::translate_transformer<double, 2, 2> translate(
    pose.position.x, pose.position.y);
  Polygon translate_obj_poly;
  bg::transform(rotate_obj_poly, translate_obj_poly, translate);
  return translate_obj_poly;
}

double getDistanceFromTwoPoint(
  const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2)
{
  const double dx = point1.x - point2.x;
  const double dy = point1.y - point2.y;
  const double dist = std::hypot(dx, dy);
  return dist;
}

[[maybe_unused]] double normalizeRadian(
  const double rad, const double min_rad = -boost::math::constants::pi<double>(),
  const double max_rad = boost::math::constants::pi<double>())
{
  const auto value = std::fmod(rad, 2 * boost::math::constants::pi<double>());
  if (min_rad < value && value <= max_rad) {
    return value;
  } else {
    return value - std::copysign(2 * boost::math::constants::pi<double>(), value);
  }
}

constexpr double sign(const double value)
{
  if (value > 0) {
    return 1.0;
  } else if (value < 0) {
    return -1.0;
  } else {
    return 0.0;
  }
}
}  // namespace

namespace motion_planning
{
AdaptiveCruiseController::AdaptiveCruiseController(
  rclcpp::Node * node, const double vehicle_width, const double vehicle_length,
  const double baselink2front)
: node_(node),
  vehicle_width_(vehicle_width),
  vehicle_length_(vehicle_length),
  baselink2front_(baselink2front)
{
  // get parameter
  std::string acc_ns = "adaptive_cruise_control.";

  /* config */
  param_.use_object_to_est_vel =
    node_->declare_parameter(acc_ns + "use_object_to_estimate_vel", true);
  param_.use_pcl_to_est_vel = node_->declare_parameter(acc_ns + "use_pcl_to_estimate_vel", true);
  param_.consider_obj_velocity = node_->declare_parameter(acc_ns + "consider_obj_velocity", true);

  /* parameter for acc */
  param_.obstacle_velocity_thresh_to_start_acc =
    node_->declare_parameter(acc_ns + "obstacle_velocity_thresh_to_start_acc", 1.5);
  param_.obstacle_velocity_thresh_to_stop_acc =
    node_->declare_parameter(acc_ns + "obstacle_velocity_thresh_to_stop_acc", 1.0);
  param_.emergency_stop_acceleration =
    node_->declare_parameter(acc_ns + "emergency_stop_acceleration", -3.5);
  param_.obstacle_emergency_stop_acceleration =
    node_->declare_parameter(acc_ns + "obstacle_emergency_stop_acceleration", -5.0);
  param_.emergency_stop_idling_time =
    node_->declare_parameter(acc_ns + "emergency_stop_idling_time", 0.5);
  param_.min_dist_stop = node_->declare_parameter(acc_ns + "min_dist_stop", 4.0);
  param_.max_standard_acceleration =
    node_->declare_parameter(acc_ns + "max_standard_acceleration", 0.5);
  param_.min_standard_acceleration =
    node_->declare_parameter(acc_ns + "min_standard_acceleration", -1.0);
  param_.standard_idling_time = node_->declare_parameter(acc_ns + "standard_idling_time", 0.5);
  param_.min_dist_standard = node_->declare_parameter(acc_ns + "min_dist_standard", 4.0);
  param_.obstacle_min_standard_acceleration =
    node_->declare_parameter(acc_ns + "obstacle_min_standard_acceleration", -1.5);
  param_.margin_rate_to_change_vel =
    node_->declare_parameter(acc_ns + "margin_rate_to_change_vel", 0.3);
  param_.use_time_compensation_to_dist =
    node_->declare_parameter(acc_ns + "use_time_compensation_to_calc_distance", true);
  param_.lowpass_gain_ = node_->declare_parameter(acc_ns + "lowpass_gain_of_upper_velocity", 0.6);

  /* parameter for pid in acc */
  param_.p_coeff_pos = node_->declare_parameter(acc_ns + "p_coefficient_positive", 0.1);
  param_.p_coeff_neg = node_->declare_parameter(acc_ns + "p_coefficient_negative", 0.3);
  param_.d_coeff_pos = node_->declare_parameter(acc_ns + "d_coefficient_positive", 0.0);
  param_.d_coeff_neg = node_->declare_parameter(acc_ns + "d_coefficient_negative", 0.1);

  /* parameter for speed estimation of obstacle */
  param_.object_polygon_length_margin =
    node_->declare_parameter(acc_ns + "object_polygon_length_margin", 2.0);
  param_.object_polygon_width_margin =
    node_->declare_parameter(acc_ns + "object_polygon_width_margin", 0.5);
  param_.valid_est_vel_diff_time =
    node_->declare_parameter(acc_ns + "valid_estimated_vel_diff_time", 1.0);
  param_.valid_vel_que_time = node_->declare_parameter(acc_ns + "valid_vel_que_time", 0.5);
  param_.valid_est_vel_max = node_->declare_parameter(acc_ns + "valid_estimated_vel_max", 20.0);
  param_.valid_est_vel_min = node_->declare_parameter(acc_ns + "valid_estimated_vel_min", -20.0);
  param_.thresh_vel_to_stop = node_->declare_parameter(acc_ns + "thresh_vel_to_stop", 0.5);
  param_.use_rough_est_vel =
    node_->declare_parameter(acc_ns + "use_rough_velocity_estimation", false);
  param_.rough_velocity_rate = node_->declare_parameter(acc_ns + "rough_velocity_rate", 0.9);

  /* publisher */
  pub_debug_ = node_->create_publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>(
    "~/adaptive_cruise_control/debug_values", 1);
}

void AdaptiveCruiseController::insertAdaptiveCruiseVelocity(
  const TrajectoryPoints & trajectory, const int nearest_collision_point_idx,
  const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point,
  const rclcpp::Time nearest_collision_point_time,
  const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr,
  const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr, bool * need_to_stop,
  TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header)
{
  debug_values_.data.clear();
  debug_values_.data.resize(num_debug_values_, 0.0);

  const double current_velocity = current_velocity_ptr->twist.twist.linear.x;
  double col_point_distance;
  double point_velocity;
  bool success_estimate_vel = false;
  /*
   * calc distance to collision point
   */
  calcDistanceToNearestPointOnPath(
    trajectory, nearest_collision_point_idx, self_pose, nearest_collision_point,
    nearest_collision_point_time, &col_point_distance, trajectory_header);

  /*
   * calc yaw of trajectory at collision point
   */
  const double traj_yaw = calcTrajYaw(trajectory, nearest_collision_point_idx);

  /*
   * estimate velocity of collision point
   */
  if (param_.use_pcl_to_est_vel) {
    if (estimatePointVelocityFromPcl(
          traj_yaw, nearest_collision_point, nearest_collision_point_time, &point_velocity)) {
      success_estimate_vel = true;
    }
  }

  if (param_.use_object_to_est_vel) {
    if (estimatePointVelocityFromObject(
          object_ptr, traj_yaw, nearest_collision_point, &point_velocity)) {
      success_estimate_vel = true;
    }
  }

  if (param_.use_rough_est_vel && !success_estimate_vel) {
    point_velocity = estimateRoughPointVelocity(current_velocity);
    success_estimate_vel = true;
  }

  if (!success_estimate_vel) {
    // if failed to estimate velocity, need to stop
    RCLCPP_DEBUG_THROTTLE(
      node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(),
      "Failed to estimate velocity of forward vehicle. Insert stop line.");
    *need_to_stop = true;
    prev_upper_velocity_ = current_velocity;  // reset prev_upper_velocity
    prev_target_velocity_ = 0.0;
    pub_debug_->publish(debug_values_);
    return;
  }

  // calculate max(target) velocity of self
  const double upper_velocity =
    calcUpperVelocity(col_point_distance, point_velocity, current_velocity);
  pub_debug_->publish(debug_values_);

  if (upper_velocity <= param_.thresh_vel_to_stop) {
    // if upper velocity is too low, need to stop
    RCLCPP_DEBUG_THROTTLE(
      node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(),
      "Upper velocity is too low. Insert stop line.");
    *need_to_stop = true;
    return;
  }

  /*
   * insert max velocity
   */
  insertMaxVelocityToPath(
    self_pose, current_velocity, upper_velocity, col_point_distance, output_trajectory);
  *need_to_stop = false;
}

void AdaptiveCruiseController::calcDistanceToNearestPointOnPath(
  const TrajectoryPoints & trajectory, const int nearest_point_idx,
  const geometry_msgs::msg::Pose & self_pose, const pcl::PointXYZ & nearest_collision_point,
  const rclcpp::Time & nearest_collision_point_time, double * distance,
  const std_msgs::msg::Header & trajectory_header)
{
  if (trajectory.size() == 0) {
    RCLCPP_DEBUG_THROTTLE(
      node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(),
      "input path is too short(size=0)");
    *distance = 0;
    return;
  }

  // get self polygon
  geometry_msgs::msg::Vector3 self_size;
  self_size.x = vehicle_length_;
  self_size.y = vehicle_width_;
  double self_offset = baselink2front_ - vehicle_length_ / 2.0;
  Polygon self_poly = getPolygon(self_pose, self_size, self_offset);

  // get nearest point
  Point nearest_point2d(nearest_collision_point.x, nearest_collision_point.y);

  if (nearest_point_idx <= 2) {
    // if too nearest collision point, return direct distance
    *distance = boost::geometry::distance(self_poly, nearest_point2d);
    debug_values_.data.at(DBGVAL::FORWARD_OBJ_DISTANCE) = *distance;
    return;
  }

  /* get total distance to collision point */
  double dist_to_point = 0;
  // get distance from self to next nearest point
  dist_to_point += boost::geometry::distance(
    convertPointRosToBoost(self_pose.position),
    convertPointRosToBoost(trajectory.at(1).pose.position));

  // add distance from next self-nearest-point(=idx:0) to prev point of nearest_point_idx
  for (int i = 1; i < nearest_point_idx - 1; i++) {
    dist_to_point +=
      getDistanceFromTwoPoint(trajectory.at(i).pose.position, trajectory.at(i + 1).pose.position);
  }

  // add distance from nearest_collision_point to prev point of nearest_point_idx
  dist_to_point += boost::geometry::distance(
    nearest_point2d, convertPointRosToBoost(trajectory.at(nearest_point_idx - 1).pose.position));

  // subtract base_link to front
  dist_to_point -= baselink2front_;

  // time compensation
  if (param_.use_time_compensation_to_dist) {
    const rclcpp::Time base_time = trajectory_header.stamp;
    double delay_time = (base_time - nearest_collision_point_time).seconds();
    dist_to_point += prev_target_velocity_ * delay_time;
  }

  *distance = std::max(0.0, dist_to_point);
  debug_values_.data.at(DBGVAL::FORWARD_OBJ_DISTANCE) = *distance;
}

double AdaptiveCruiseController::calcTrajYaw(
  const TrajectoryPoints & trajectory, const int collision_point_idx)
{
  return tf2::getYaw(trajectory.at(collision_point_idx).pose.orientation);
}

bool AdaptiveCruiseController::estimatePointVelocityFromObject(
  const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr,
  const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, double * velocity)
{
  geometry_msgs::msg::Point nearest_collision_p_ros;
  nearest_collision_p_ros.x = nearest_collision_point.x;
  nearest_collision_p_ros.y = nearest_collision_point.y;
  nearest_collision_p_ros.z = nearest_collision_point.z;

  /* get object velocity, and current yaw */
  bool get_obj = false;
  double obj_vel;
  double obj_yaw;
  const Point collision_point_2d = convertPointRosToBoost(nearest_collision_p_ros);
  for (const auto & obj : object_ptr->objects) {
    const Polygon obj_poly = getPolygon(
      obj.kinematics.initial_pose_with_covariance.pose, obj.shape.dimensions, 0.0,
      param_.object_polygon_length_margin, param_.object_polygon_width_margin);
    if (boost::geometry::distance(obj_poly, collision_point_2d) <= 0) {
      obj_vel = obj.kinematics.initial_twist_with_covariance.twist.linear.x;
      obj_yaw = tf2::getYaw(obj.kinematics.initial_pose_with_covariance.pose.orientation);
      get_obj = true;
      break;
    }
  }

  if (get_obj) {
    *velocity = obj_vel * std::cos(obj_yaw - traj_yaw);
    debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity;
    return true;
  } else {
    return false;
  }
}

bool AdaptiveCruiseController::estimatePointVelocityFromPcl(
  const double traj_yaw, const pcl::PointXYZ & nearest_collision_point,
  const rclcpp::Time & nearest_collision_point_time, double * velocity)
{
  geometry_msgs::msg::Point nearest_collision_p_ros;
  nearest_collision_p_ros.x = nearest_collision_point.x;
  nearest_collision_p_ros.y = nearest_collision_point.y;
  nearest_collision_p_ros.z = nearest_collision_point.z;

  /* estimate velocity */
  const double p_dt = nearest_collision_point_time.seconds() - prev_collision_point_time_.seconds();

  // if get same pointcloud with previous step,
  // skip estimate process
  if (std::fabs(p_dt) > std::numeric_limits<double>::epsilon()) {
    // valid time check
    if (p_dt < 0 || param_.valid_est_vel_diff_time < p_dt) {
      prev_collision_point_time_ = nearest_collision_point_time;
      prev_collision_point_ = nearest_collision_point;
      prev_collision_point_valid_ = true;
      return false;
    }
    const double p_dx = nearest_collision_point.x - prev_collision_point_.x;
    const double p_dy = nearest_collision_point.y - prev_collision_point_.y;
    const double p_dist = std::hypot(p_dx, p_dy);
    const double p_yaw = std::atan2(p_dy, p_dx);
    const double p_vel = p_dist / p_dt;
    const double est_velocity = p_vel * std::cos(p_yaw - traj_yaw);
    // valid velocity check
    if (est_velocity <= param_.valid_est_vel_min || param_.valid_est_vel_max <= est_velocity) {
      prev_collision_point_time_ = nearest_collision_point_time;
      prev_collision_point_ = nearest_collision_point;
      prev_collision_point_valid_ = true;
      est_vel_que_.clear();
      return false;
    }

    // append new velocity and remove old velocity from que
    registerQueToVelocity(est_velocity, nearest_collision_point_time);
  }

  // calc average(median) velocity from que
  *velocity = getMedianVel(est_vel_que_);
  debug_values_.data.at(DBGVAL::ESTIMATED_VEL_PCL) = *velocity;

  prev_collision_point_time_ = nearest_collision_point_time;
  prev_collision_point_ = nearest_collision_point;
  prev_target_velocity_ = *velocity;
  prev_collision_point_valid_ = true;
  return true;
}

double AdaptiveCruiseController::estimateRoughPointVelocity(double current_vel)
{
  const double p_dt = node_->now().seconds() - prev_collision_point_time_.seconds();
  if (param_.valid_est_vel_diff_time >= p_dt) {
    // use previous estimated velocity
    return prev_target_velocity_;
  }

  // use current velocity * rough velocity rate
  return current_vel * param_.rough_velocity_rate;
}

bool AdaptiveCruiseController::isObstacleVelocityHigh(const double obj_vel)
{
  bool is_high = false;
  if (prev_obstacle_velocity_judge_to_start_acc_) {
    is_high = obj_vel > param_.obstacle_velocity_thresh_to_stop_acc;
  } else {
    is_high = obj_vel > param_.obstacle_velocity_thresh_to_start_acc;
  }
  prev_obstacle_velocity_judge_to_start_acc_ = is_high;
  return is_high;
}

double AdaptiveCruiseController::calcUpperVelocity(
  const double dist_to_col, const double obj_vel, const double self_vel)
{
  debug_values_.data.at(DBGVAL::ESTIMATED_VEL_FINAL) = obj_vel;
  if (!isObstacleVelocityHigh(obj_vel)) {
    // stop acc by low-velocity obstacle
    RCLCPP_DEBUG_THROTTLE(
      node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(),
      "The velocity of forward vehicle is too low. Insert stop line.");
    prev_upper_velocity_ = self_vel;  // reset prev_upper_velocity
    return 0.0;
  }

  const double thresh_dist = calcThreshDistToForwardObstacle(self_vel, obj_vel);
  if (thresh_dist >= dist_to_col) {
    // emergency stop
    RCLCPP_DEBUG_THROTTLE(
      node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(),
      "Forward vehicle is too close. Insert stop line.");
    prev_upper_velocity_ = self_vel;  // reset prev_upper_velocity
    return 0.0;
  }

  const double upper_velocity =
    std::max(1e-01, calcTargetVelocityByPID(self_vel, dist_to_col, obj_vel));
  const double lowpass_upper_velocity =
    lowpass_filter(upper_velocity, prev_upper_velocity_, param_.lowpass_gain_);
  prev_upper_velocity_ = lowpass_upper_velocity;
  debug_values_.data.at(DBGVAL::UPPER_VEL) = lowpass_upper_velocity;
  return lowpass_upper_velocity;
}

double AdaptiveCruiseController::calcThreshDistToForwardObstacle(
  const double current_vel, const double obj_vel)
{
  const double current_vel_min = std::max(1.0, std::fabs(current_vel));
  const double obj_vel_min = std::max(0.0, obj_vel);
  const double minimum_distance = param_.min_dist_stop;
  const double idling_distance = current_vel_min * param_.emergency_stop_idling_time;
  const double braking_distance =
    (-1.0 * current_vel_min * current_vel_min) / (2.0 * param_.emergency_stop_acceleration);
  const double obj_braking_distance =
    (-1.0 * obj_vel_min * obj_vel_min) / (2.0 * param_.obstacle_emergency_stop_acceleration);

  return minimum_distance + std::max(
                              0.0, idling_distance + braking_distance -
                                     obj_braking_distance * param_.consider_obj_velocity);
}

double AdaptiveCruiseController::calcBaseDistToForwardObstacle(
  const double current_vel, const double obj_vel)
{
  const double obj_vel_min = std::max(0.0, obj_vel);
  const double minimum_distance = param_.min_dist_standard;
  const double idling_distance = current_vel * param_.standard_idling_time;
  const double braking_distance =
    (-1.0 * current_vel * current_vel) / (2.0 * param_.min_standard_acceleration);
  const double obj_braking_distance =
    (-1.0 * obj_vel_min * obj_vel_min) / (2.0 * param_.obstacle_min_standard_acceleration);
  return minimum_distance + std::max(
                              0.0, idling_distance + braking_distance -
                                     obj_braking_distance * param_.consider_obj_velocity);
}

double AdaptiveCruiseController::calcTargetVelocity_P(
  const double target_dist, const double current_dist)
{
  const double diff_dist = current_dist - target_dist;
  double add_vel_p;
  if (diff_dist >= 0) {
    add_vel_p = diff_dist * param_.p_coeff_pos;
  } else {
    add_vel_p = diff_dist * param_.p_coeff_neg;
  }
  return add_vel_p;
}

double AdaptiveCruiseController::calcTargetVelocity_I(
  [[maybe_unused]] const double target_dist, [[maybe_unused]] const double current_dist)
{
  // not implemented
  return 0.0;
}

double AdaptiveCruiseController::calcTargetVelocity_D(
  const double target_dist, const double current_dist)
{
  if (node_->now().seconds() - prev_target_vehicle_time_ >= param_.d_coeff_valid_time) {
    // invalid time(prev is too old)
    return 0.0;
  }

  double diff_vel = (target_dist - prev_target_vehicle_dist_) /
                    (node_->now().seconds() - prev_target_vehicle_time_);

  if (std::fabs(diff_vel) >= param_.d_coeff_valid_diff_vel) {
    // invalid(discontinuous) diff_vel
    return 0.0;
  }

  double add_vel_d = 0;

  add_vel_d = diff_vel;
  if (add_vel_d >= 0) {
    diff_vel *= param_.d_coeff_pos;
  }
  if (add_vel_d < 0) {
    diff_vel *= param_.d_coeff_neg;
  }
  add_vel_d = boost::algorithm::clamp(add_vel_d, -param_.d_max_vel_norm, param_.d_max_vel_norm);

  // add buffer
  prev_target_vehicle_dist_ = current_dist;
  prev_target_vehicle_time_ = node_->now().seconds();

  return add_vel_d;
}

double AdaptiveCruiseController::calcTargetVelocityByPID(
  const double current_vel, const double current_dist, const double obj_vel)
{
  const double target_dist = calcBaseDistToForwardObstacle(current_vel, obj_vel);
  RCLCPP_DEBUG_STREAM(node_->get_logger(), "[adaptive cruise control] target_dist" << target_dist);

  const double add_vel_p = calcTargetVelocity_P(target_dist, current_dist);
  //** I is not implemented **
  const double add_vel_i = calcTargetVelocity_I(target_dist, current_dist);
  const double add_vel_d = calcTargetVelocity_D(target_dist, current_dist);

  double target_vel = current_vel + add_vel_p + add_vel_i + add_vel_d;
  debug_values_.data.at(DBGVAL::CURRENT_VEL) = current_vel;
  debug_values_.data.at(DBGVAL::UPPER_VEL_P) = add_vel_p;
  debug_values_.data.at(DBGVAL::UPPER_VEL_I) = add_vel_i;
  debug_values_.data.at(DBGVAL::UPPER_VEL_D) = add_vel_d;
  debug_values_.data.at(DBGVAL::UPPER_VEL_RAW) = target_vel;
  return target_vel;
}

void AdaptiveCruiseController::insertMaxVelocityToPath(
  const geometry_msgs::msg::Pose self_pose, const double current_vel, const double target_vel,
  const double dist_to_collision_point, TrajectoryPoints * output_trajectory)
{
  // plus distance from self to next nearest point
  double dist = dist_to_collision_point;
  double dist_to_first_point = 0.0;
  if (output_trajectory->size() > 1) {
    dist_to_first_point = boost::geometry::distance(
      convertPointRosToBoost(self_pose.position),
      convertPointRosToBoost(output_trajectory->at(1).pose.position));
  }
  dist += dist_to_first_point;

  double margin_to_insert = dist_to_collision_point * param_.margin_rate_to_change_vel;
  // accel = (v_after^2 - v_before^2 ) / 2x
  double target_acc = (std::pow(target_vel, 2) - std::pow(current_vel, 2)) / (2 * margin_to_insert);

  const double clipped_acc = boost::algorithm::clamp(
    target_acc, param_.min_standard_acceleration, param_.max_standard_acceleration);
  double pre_vel = current_vel;
  double total_dist = 0.0;
  for (size_t i = 1; i < output_trajectory->size(); i++) {
    // calc velocity of each point by gradient deceleration
    const auto current_p = output_trajectory->at(i);
    const auto prev_p = output_trajectory->at(i - 1);
    const double p_dist = getDistanceFromTwoPoint(current_p.pose.position, prev_p.pose.position);
    total_dist += p_dist;
    if (current_p.longitudinal_velocity_mps > target_vel && total_dist >= 0) {
      double next_pre_vel;
      if (std::fabs(clipped_acc) < 1e-05) {
        next_pre_vel = pre_vel;
      } else {
        // v_after = sqrt (2x*accel + v_before^2)
        next_pre_vel = std::sqrt(2 * p_dist * clipped_acc + std::pow(pre_vel, 2));
      }
      if (target_acc >= 0) {
        next_pre_vel = std::min(next_pre_vel, target_vel);
      } else {
        next_pre_vel = std::max(next_pre_vel, target_vel);
      }

      if (total_dist >= margin_to_insert) {
        const double max_velocity = std::max(target_vel, next_pre_vel);
        if (output_trajectory->at(i).longitudinal_velocity_mps > max_velocity) {
          output_trajectory->at(i).longitudinal_velocity_mps = max_velocity;
        }
      }
      pre_vel = next_pre_vel;
    }
  }
}

void AdaptiveCruiseController::registerQueToVelocity(
  const double vel, const rclcpp::Time & vel_time)
{
  // remove old msg from que
  std::vector<int> delete_idxs;
  for (size_t i = 0; i < est_vel_que_.size(); i++) {
    if (node_->now().seconds() - est_vel_que_.at(i).header.stamp.sec > param_.valid_vel_que_time) {
      delete_idxs.push_back(i);
    }
  }
  for (int delete_idx = delete_idxs.size() - 1; 0 <= delete_idx; --delete_idx) {
    est_vel_que_.erase(est_vel_que_.begin() + delete_idxs.at(delete_idx));
  }

  // append new que
  nav_msgs::msg::Odometry new_vel;
  new_vel.header.stamp = vel_time;
  new_vel.twist.twist.linear.x = vel;
  est_vel_que_.emplace_back(new_vel);
}

double AdaptiveCruiseController::getMedianVel(const std::vector<nav_msgs::msg::Odometry> vel_que)
{
  if (vel_que.size() == 0) {
    RCLCPP_WARN_STREAM(node_->get_logger(), "size of vel que is 0. Something has wrong.");
    return 0.0;
  }

  std::vector<double> raw_vel_que;
  for (const auto & vel : vel_que) {
    raw_vel_que.emplace_back(vel.twist.twist.linear.x);
  }

  double med_vel;
  if (raw_vel_que.size() % 2 == 0) {
    size_t med1 = (raw_vel_que.size()) / 2 - 1;
    size_t med2 = (raw_vel_que.size()) / 2;
    std::nth_element(raw_vel_que.begin(), raw_vel_que.begin() + med1, raw_vel_que.end());
    const double vel1 = raw_vel_que[med1];
    std::nth_element(raw_vel_que.begin(), raw_vel_que.begin() + med2, raw_vel_que.end());
    const double vel2 = raw_vel_que[med2];
    med_vel = (vel1 + vel2) / 2;
  } else {
    size_t med = (raw_vel_que.size() - 1) / 2;
    std::nth_element(raw_vel_que.begin(), raw_vel_que.begin() + med, raw_vel_que.end());
    med_vel = raw_vel_que[med];
  }

  return med_vel;
}

double AdaptiveCruiseController::lowpass_filter(
  const double current_value, const double prev_value, const double gain)
{
  return gain * prev_value + (1.0 - gain) * current_value;
}

}  // namespace motion_planning


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_FIFTEEN_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_FIFTEEN_HPP_INCLUDED

/*!
  @ingroup group-constant
  @defgroup constant-Fifteen Fifteen (function template)

  Generates the constant @c 15.

  @headerref{<boost/simd/constant/fifteen.hpp>}

  @par Description

  1.  @code
      template<typename T> T Fifteen();
      @endcode

  2.  @code
      template<typename T> T Fifteen( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T that evaluates to 15.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that evaluates to `T(15)`.

  @par Requirements
  - **T** models Value
**/

#include <boost/simd/constant/scalar/fifteen.hpp>
#include <boost/simd/constant/simd/fifteen.hpp>

#endif