#include "TilePartitionerNoise.h"
#include "logging/Logging.h"
#include "noise/RasterValues.h"
#include "noise/module/Pow.h"
#include "tilegen/alpha/CalculatorMax.h"
#include "tilegen/alpha/CalculatorLinear.h"
#include "tilegen/alpha/CalculatorTopTwo.h"
#include "tilegen/alpha/CalculatorDither.h"
#include "noise/RasterImage.h"
#include "noise/ModuleGroup.h"
#include <boost/filesystem.hpp>

#include <iostream>

namespace tilegen
{
namespace partition
{

noise::module::ModulePtr TilePartitionerNoise::makeCornerModule(const Corners& corners,
                                                                bool left, bool top)
{
    TerrainID corner_id = corners[ (left ? 0 : 2) +  (top ? 0 : 1)];
    TerrainID corner_h =  corners[(!left ? 0 : 2) +  (top ? 0 : 1)];
    TerrainID corner_v =  corners[ (left ? 0 : 2) + (!top ? 0 : 1)];

    noise::ModuleGroup& combiner = mNoiseModuleManager.getCombiner();
    noise::ModuleGroup& central = mNoiseModuleManager.getCentral(corner_id);
    noise::ModuleGroup& border_h =
        left
        ? mNoiseModuleManager.getLeftBorder(left ? corner_id : corner_h,
                                            left ? corner_h : corner_id)
        : mNoiseModuleManager.getBottomRightBorder();
    noise::ModuleGroup& border_v =
        top
        ? mNoiseModuleManager.getTopBorder(top ? corner_id : corner_v,
                                           top ? corner_v : corner_id)
        : mNoiseModuleManager.getBottomRightBorder();
    central.setQuadrant(left, top, false);
    border_h.setQuadrant(left, top, false);
    border_v.setQuadrant(left, top, false);
    combiner.setQuadrant(left, top, false);

    combiner.setInputModuleSource(0, border_h.getOutputModule());
    combiner.setInputModuleSource(1, border_v.getOutputModule());
    combiner.setInputModuleSource(2, central.getOutputModule());
    if (mDebugOutput)
    {
        writeDebugGroup(combiner, Combiner, top, left);
        writeDebugGroup(border_h, HorizontalBorder, top, left);
        writeDebugGroup(border_v, VerticalBorder, top, left);
        writeDebugGroup(central, Central, top, left);
    }
    return combiner.getOutputModule();
}

void TilePartitionerNoise::noiseToAlpha(std::vector<noise::RasterValues<double>>& noise_values,
                                        std::vector<sf::Image>& outputs,
                                        sf::Vector2u resolution) const
{
    std::vector<double> weights((int)CORNERS);
    std::unique_ptr<alpha::CalculatorBase> ac;
    switch (mOptions.calculatorMode)
    {
    case alpha::CalculatorMode::Max:
        ac = std::make_unique<alpha::CalculatorMax>();
        break;
    case alpha::CalculatorMode::Linear:
        ac = std::make_unique<alpha::CalculatorLinear>();
        break;
    case alpha::CalculatorMode::TopTwo:
    {
        auto ac_top_two = std::make_unique<alpha::CalculatorTopTwo>();
        if (mOptions.alphaCalculatorTopTwoPower)
            ac_top_two->power = mOptions.alphaCalculatorTopTwoPower.get();
        ac = std::move(ac_top_two);
        break;
    }
    case alpha::CalculatorMode::Dither:
    {
        auto ac_dither = std::make_unique<alpha::CalculatorDither>();
        if (mOptions.alphaCalculatorTopTwoPower)
            ac_dither->power = mOptions.alphaCalculatorTopTwoPower.get();
        ac = std::move(ac_dither);
        break;
    }
    default:
        throw std::runtime_error("Invalid CalculatorMode");
    }
    for (size_t x = 0; x < resolution.x; x++)
    {
        for (size_t y = 0; y < resolution.y; y++)
        {
            for (int i = 0; i < (int)CORNERS; i++)
            {
                weights[i] = noise_values[i].get(x, y);
            }

            ac->updateAlphas(weights);
            const auto& alphas = ac->getAlphas();
            for (int i = 0; i < (int)CORNERS; i++)
            {
                outputs[i].setPixel(x, y, sf::Color(255, 255, 255, alphas[i]));
            }
        }
    }
}

void TilePartitionerNoise::writeDebugGroup(const noise::ModuleGroup& module_group, tilegen::ModuleGroupRole module_group_role, bool top, bool left)
{
    if (!mDebugModuleWriter)
    {
        logError() << "Requested debug data but didn't provide a debug module writer function!\n";
        throw std::runtime_error("Unable to write debug data");
    }
    for (auto it : module_group.getModules())
    {
        mDebugModuleWriter(tilegen::DebugTilesetID(module_group_role, it.first, top, left), it.second);
    }
}

void TilePartitionerNoise::makePartition(TilePartition & regions, const Corners& corners)
{
    // Prepare noise value storage
    std::vector<noise::RasterValues<double>> noise_values;
    for (int i = 0; i < (int)CORNERS; i++)
    {
        noise_values.emplace_back(mOptions.tileFormat.resolution.x,
                          mOptions.tileFormat.resolution.y,
                          sf::Rect<double>{0, 0, 1, 1});
    }
    // Construct noise modules and render them.
    // Construction and rendering must be done in the same step,
    // because module seeds will be overwritten.
    noise::module::ModulePtr corner_module;
    for (int i = 0; i < 2; i++)
        for (int j = 0; j < 2; j++)
        {
            corner_module = makeCornerModule(corners, i == 0, j == 0);
            int k = (2 * i) + j;
            noise_values[k].build(corner_module->getModule());
        }
    // Prepare output storage
    std::vector<sf::Image> outputs((int)CORNERS);
    for (int i = 0; i < (int)CORNERS; i++)
        outputs[i].create(mOptions.tileFormat.resolution.x, mOptions.tileFormat.resolution.y);
    // Convert noise values to alpha values
    noiseToAlpha(noise_values, outputs, mOptions.tileFormat.resolution);
    // Convert output images to required format
    for (int i = 0; i < (int)CORNERS; i++)
    {
        sf::Texture t;
        t.loadFromImage(outputs[i]);
        regions.push_back({t, corners[i]});
    }
}

TilePartitionerNoise::TilePartitionerNoise(const Options & options) :
    TilePartitionerBase(options),
    mNoiseModuleManager(options),
    mDebugOutput(options.debugOutput)
{
}

} // namespace partition
} // namespace tilegen


#include "wer.h"

#include <cstdio>
#include <cassert>
#include <iostream>
#include <limits>
#include <sstream>
#ifndef HAVE_OLD_CPP
# include <unordered_map>
#else
# include <tr1/unordered_map>
namespace std { using std::tr1::unordered_map; }
#endif
#include <set>
#include <valarray>
#include <boost/functional/hash.hpp>
#include <stdexcept>
#include "tdict.h"
#include "levenshtein.h"

using namespace std;

class WERScore : public ScoreBase<WERScore> {
  friend class WERScorer;

 public:
  static const unsigned kEDITDISTANCE = 0;
  static const unsigned kCHARCOUNT = 1;
  static const unsigned kDUMMY_LAST_ENTRY = 2;

 WERScore() : stats(0,kDUMMY_LAST_ENTRY) {}
  float ComputePartialScore() const { return 0.0;}
  float ComputeScore() const {
    return static_cast<float>(stats[kEDITDISTANCE]) / static_cast<float>(stats[kCHARCOUNT]);
  }
  void ScoreDetails(string* details) const;
  void PlusPartialEquals(const Score& rhs, int oracle_e_cover, int oracle_f_cover, int src_len){}
  void PlusEquals(const Score& delta, const float scale) {
    if (scale==1)
      stats += static_cast<const WERScore&>(delta).stats;
    if (scale==-1)
      stats -= static_cast<const WERScore&>(delta).stats;
    throw std::runtime_error("WERScore::PlusEquals with scale != +-1");
 }
  void PlusEquals(const Score& delta) {
    stats += static_cast<const WERScore&>(delta).stats;
  }

  ScoreP GetZero() const {
    return ScoreP(new WERScore);
  }
  ScoreP GetOne() const {
    return ScoreP(new WERScore);
  }
  void Subtract(const Score& rhs, Score* res) const {
    static_cast<WERScore*>(res)->stats = stats - static_cast<const WERScore&>(rhs).stats;
  }
  void Encode(std::string* out) const {
    ostringstream os;
    os << stats[kEDITDISTANCE] << ' '
       << stats[kCHARCOUNT];
    *out = os.str();
  }
  bool IsAdditiveIdentity() const {
    for (int i = 0; i < kDUMMY_LAST_ENTRY; ++i)
      if (stats[i] != 0) return false;
    return true;
  }
 private:
  valarray<int> stats;
};

ScoreP WERScorer::ScoreFromString(const std::string& data) {
  istringstream is(data);
  WERScore* r = new WERScore;
  is >> r->stats[WERScore::kEDITDISTANCE]
     >> r->stats[WERScore::kCHARCOUNT];
  return ScoreP(r);
}

void WERScore::ScoreDetails(std::string* details) const {
  char buf[200];
  sprintf(buf, "WER = %.2f, edits=%d, len=%d",
     ComputeScore() * 100.0f,
     stats[kEDITDISTANCE],
     stats[kCHARCOUNT]);
  *details = buf;
}

WERScorer::~WERScorer() {}
WERScorer::WERScorer(const vector<vector<WordID> >& refs) {}

ScoreP WERScorer::ScoreCCandidate(const vector<WordID>& hyp) const {
  return ScoreP();
}

float WERScorer::Calculate(const std::vector<WordID>& hyp, const Sentence& ref, int& edits, int& char_count) const {
  edits = cdec::LevenshteinDistance(hyp, ref);
  char_count = ref.size();
  return static_cast<float>(edits) / static_cast<float>(char_count);
}

ScoreP WERScorer::ScoreCandidate(const std::vector<WordID>& hyp) const {
  float best_score = numeric_limits<float>::max();
  WERScore* res = new WERScore;
  for (int i = 0; i < refs.size(); ++i) {
    int edits, char_count;
    const vector<WordID>& ref = refs[i];
    float score = Calculate(hyp, ref, edits, char_count);
    if (score < best_score) {
      res->stats[WERScore::kEDITDISTANCE] = edits;
      res->stats[WERScore::kCHARCOUNT] = char_count;
      best_score = score;
    }
  }
  return ScoreP(res);
}


#include "colibri_ca.h"
#include "colibri_local_nav.h"
#include "colibri_action.h"
#include "PID_controller.h"
#include "global_planner.h"

#include <boost/bind.hpp>

void PlannerCallback(planner *plannerObj, float* start_pos, float* goal_pos, bool *finish_flag);

int main(int argc, char* argv[])
{
	ros::init(argc, argv, "Follow_path_node");

	scan_ca scan4caObj;
	local_nav local4navObj;
	nav_action actionObj;
	planner plannerObj;

	ros::Rate loop_rate(10);

	ROS_INFO("Start to Follow a path ... ");

	unsigned int delay_cnt = 0;

	int range_num = 0;
	float tmp_theta_laseray = 0.0;	//obstacle 's theta angle

	float tmp_delta_dis = 0.0;
	float tmp_robot2goal_yaw = 0.0;
	float tmp_laser2goal_yaw = 0.0;
	bool goal_inlaser_flag = true;
	float dir_goal_in_laser = 0.0;
	float self_rotation_angle = 0.0;

	unsigned int turn_adj_flag = 1;

	float tmp_action_cmd_t[2] = {0.0, 0.0};
	float* ptr_action_cmd_t = tmp_action_cmd_t;
	float newyaw = 145.0;
	unsigned int micro_adj_flag = 0;

	unsigned int adj_flag = 0;

	bool obtain_flag = false;
	unsigned int search_start = 0;

	ros::NodeHandle nh_fp;
	ros::Timer planner_timer;
	bool finish_plan = false;

	bool tmp_timer_finish =false;
	bool *timer_finish = &tmp_timer_finish;

	static unsigned int index4gravaton = 0;

	float delta_robot2gravaton = 0.0;
	bool at_gravaton_flag = false;
	bool exist_gravaton_flag = false;
	bool replan_flag = false;

	float rt_r2g_dis = 100.0;

	while (!ros::service::waitForService(plannerObj.srv4make_plan, ros::Duration(0.2)))
	{
		ROS_INFO("Waiting for service move_base/make_plan to become available");
	}

	ROS_INFO("Service move_base/make_plan prepared OK...");

	finish_plan = plannerObj.ExecMonoPlanAndGravaton(plannerObj,&local4navObj.cur_robot_state[0],&local4navObj.goal_state[0], search_start,index4gravaton);

	planner_timer = nh_fp.createTimer(ros::Duration(PLAN_INTERVAL), boost::bind(&PlannerCallback, &plannerObj, &local4navObj.cur_robot_state[0],&local4navObj.goal_state[0], timer_finish));

	while (ros::ok())
	{

		if(delay_cnt < DELAY_CNT_MAX)
		{
			delay_cnt++;
			ros::spinOnce();
			loop_rate.sleep();
		}

		if(delay_cnt >= DELAY_CNT_MAX)
		{
			ROS_INFO("------ start ------");
			if(*timer_finish == true)
			{
				*timer_finish = false;
				index4gravaton = 0;
				replan_flag = true;

			}else
			{

				if((at_gravaton_flag == true && local4navObj.approaching_flag == false)||(replan_flag == true))
				{
					plannerObj.CalcPath2RobotDeltaDis(plannerObj.path_array, local4navObj.cur_robot_state);
					index4gravaton = plannerObj.CalcGravatonFromPath(plannerObj.path_array, plannerObj.path2robot_array, index4gravaton, plannerObj.gravaton,exist_gravaton_flag);
					at_gravaton_flag = false;

					replan_flag = false;

				}

				cout<<" Waiting for next path plan..."<<endl;

				cout<<" plannerObj.gravation.x = "<<plannerObj.gravaton.x<<endl;
				cout<<" plannerObj.gravation.y = "<<plannerObj.gravaton.y<<endl;

				at_gravaton_flag = actionObj.ReachGravatonOK(&local4navObj.cur_robot_state[0],&plannerObj.gravaton.x, delta_robot2gravaton);
				cout<<" delta_robot2gravaton = "<<delta_robot2gravaton<<endl;
				cout<<" at_gravaton_flag : "<<at_gravaton_flag<<endl;

				if(local4navObj.approaching_flag == true)
				{
					plannerObj.gravaton.x = local4navObj.goal_state[0];
					plannerObj.gravaton.y = local4navObj.goal_state[1];
					plannerObj.gravaton.yaw = local4navObj.goal_state[2];

				}
			}

			local4navObj.CalcOffsetOfGoalAndRobot(local4navObj.cur_robot_state, &plannerObj.gravaton.x, &tmp_delta_dis, &tmp_robot2goal_yaw, &tmp_laser2goal_yaw);

			goal_inlaser_flag = local4navObj.CalcGoalDirOfLaserView(&tmp_laser2goal_yaw, &local4navObj.cur_robot_state[2], &dir_goal_in_laser, &self_rotation_angle);

			for(int i = 0; i < NUM_RAY4CA; i++)
			{
				scan4caObj.delta_phi_vec[i] = asin(D_SF / (*(scan4caObj.ptrScan4ca + i))) * RAD2DEG;
				scan4caObj.kp_phi_vec[i] = scan4caObj.CalcKpPhi(local4navObj.cur_robot_vel[0], *(scan4caObj.ptrScan4ca + i));
				range_num = floor(scan4caObj.delta_phi_vec[i] / RAY_RESOL4CA);

				scan4caObj.CalcPhiRange(i,range_num,&scan4caObj.phi_start_vec[i-1],&scan4caObj.phi_end_vec[i-1]);

				tmp_theta_laseray = i; 			// unit in degree
				scan4caObj.kaf_vec[i] = cos((tmp_theta_laseray - dir_goal_in_laser) * DEG2RAD);  //dir_goal_in_laser is 0~180 degree from right side
				scan4caObj.krf_vec[i] = scan4caObj.kp_phi_vec[i];
			}

			scan4caObj.CalcKrfTheta(scan4caObj.kp_phi_vec, scan4caObj.phi_start_vec, scan4caObj.phi_end_vec);
			scan4caObj.CalcCorrectedKrf();
			scan4caObj.CalcPassFcnAndFwdBnd(scan4caObj.wander, &scan4caObj.max_passfcn_val, scan4caObj.passfcn_vec);
			scan4caObj.CalcPassFcnAndBwdBnd(scan4caObj.wander, &scan4caObj.max_passfcn_val, scan4caObj.passfcn_vec);

			scan4caObj.angle_adj = scan4caObj.CalcAdjDir(scan4caObj.passfcn_vec,scan4caObj.max_passfcn_val, &scan4caObj.maxfcn_fwdbnd,&scan4caObj.maxfcn_bwdbnd);

			scan4caObj.CalcAlarmInAPF();

			if(goal_inlaser_flag == true)
			{
				local4navObj.apf_ctrl_output[0] = (V_MAX - V_MIN) * (scan4caObj.max_passfcn_val / D_M) + V_MIN;
				local4navObj.apf_ctrl_output[1] = scan4caObj.angle_adj / 200.0;
			}
			else
			{
				local4navObj.apf_ctrl_output[0] = 0.0;
				local4navObj.apf_ctrl_output[1] = 0.0;
			}

			local4navObj.SatuateCmdVel(local4navObj.apf_ctrl_output,local4navObj.apf_ctrl_output+1);

			local4navObj.CalcEuclidDistance(local4navObj.cur_robot_state, local4navObj.goal_state, rt_r2g_dis);

			local4navObj.approaching_flag = local4navObj.ReachApprochingAreaOK(&rt_r2g_dis);

			if(tmp_delta_dis >= GOAL_NGHBORHD)
			{
				ptr_action_cmd_t = actionObj.AdjustMovingDirAction(&local4navObj.cur_robot_state[2], &dir_goal_in_laser, &tmp_robot2goal_yaw, &turn_adj_flag);
			}
			else
			{

			}

//			cout<<"turn_adj_flag: " << turn_adj_flag <<endl;

			if(turn_adj_flag == 1)
			{
				if(local4navObj.approaching_flag == false)
				{
					*ptr_action_cmd_t = local4navObj.apf_ctrl_output[0];
					*(ptr_action_cmd_t + 1) = local4navObj.apf_ctrl_output[1];
				}
				else
				{
					ptr_action_cmd_t = actionObj.ApproachingGoalAction(&local4navObj.cur_robot_state[0],&local4navObj.goal_state[0],&dir_goal_in_laser,&micro_adj_flag);
				}

			}

			cout<<"current_robot_state[0]"<<local4navObj.cur_robot_state[0]<<endl;
			cout<<"current_robot_state[1]"<<local4navObj.cur_robot_state[1]<<endl;
			cout<<"current_robot_state[2]"<<local4navObj.cur_robot_state[2]<<endl;

			cout<<"adj_angle: "<< scan4caObj.angle_adj <<endl;

			cout<<"tmp_delta_dis: "<<tmp_delta_dis<<endl;
			cout<<"tmp_robot2goal_yaw: "<<tmp_robot2goal_yaw<<endl;
			cout<<"tmp_laser2goal_yaw: "<<tmp_laser2goal_yaw<<endl;

			cout<<"dir_goal_in_laser: "<<dir_goal_in_laser<<endl;

			//local4navObj.position_OK_flag = local4navObj.ReachGoalPositionOK(&tmp_delta_dis);

			local4navObj.SatuateCmdVel(ptr_action_cmd_t,ptr_action_cmd_t + 1);

			local4navObj.apf_cmd_vel.linear.x = *ptr_action_cmd_t;
			local4navObj.apf_cmd_vel.angular.z = *(ptr_action_cmd_t + 1);

			if(local4navObj.position_OK_flag == true)
			{
				local4navObj.apf_cmd_vel.linear.x = 0.0;
				local4navObj.apf_cmd_vel.angular.z = 0.0;
			}

			cout<<"approaching_flag: " << local4navObj.approaching_flag <<endl;
			cout<<"position_OK_flag: " << local4navObj.position_OK_flag <<endl;

			local4navObj.pub_apf_twist.publish(local4navObj.apf_cmd_vel);
			cout<<"pub_linear_x: " << local4navObj.apf_cmd_vel.linear.x <<endl;
			cout<<"pub_angular_z: " << local4navObj.apf_cmd_vel.angular.z <<endl;

			scan4caObj.ResetMaxPassValCnt();

			ros::spinOnce();
			loop_rate.sleep();
			ROS_INFO("------- end --------");
		}
	}

	local4navObj.apf_cmd_vel.linear.x = 0.0;
	local4navObj.apf_cmd_vel.angular.z = 0.0;

	local4navObj.pub_apf_twist.publish(local4navObj.apf_cmd_vel);

	return 0;
}

void PlannerCallback(planner *plannerObj, float* start_pos, float* goal_pos, bool *finish_flag)
{
	plannerObj->ObtainPathArray(plannerObj->serviceClient, plannerObj->path_srv,start_pos,goal_pos,finish_flag);
}


// Copyright (c) 2021 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE CTCLossPlanTest
#include <boost/test/unit_test.hpp>

#include <poplibs_support/TestDevice.hpp>
#include <popnn/CTCInference.hpp>
#include <popnn/CTCLoss.hpp>

#include <poputil/exceptions.hpp>

// Common simple plan parameters
const poplar::Type inType = poplar::FLOAT;
const poplar::Type outType = poplar::FLOAT;
constexpr unsigned batchSize = 10;
constexpr unsigned maxTime = 40;
constexpr unsigned maxLabelLength = 10;
constexpr unsigned numClasses = 4;

BOOST_AUTO_TEST_CASE(SimplePlan) {
  auto device = createTestDeviceFullSize(TEST_TARGET, 1);
  auto &target = device.getTarget();
  poplar::Graph graph(target);

  // Finds reasonably sized plan
  popnn::ctc::plan(graph, inType, outType, batchSize, maxTime, maxLabelLength,
                   numClasses, {{"availableMemoryProportion", "0.9"}});
}

BOOST_AUTO_TEST_CASE(SimplePlanWithMemoryBound) {
  auto device = createTestDeviceFullSize(TEST_TARGET, 1);
  auto &target = device.getTarget();
  poplar::Graph graph(target);

  // Can't find plan when no available memory
  BOOST_CHECK_THROW(popnn::ctc::plan(graph, inType, outType, batchSize, maxTime,
                                     maxLabelLength, numClasses,
                                     {{"availableMemoryProportion", "0.0"}}),
                    poputil::poplibs_error);
}

BOOST_AUTO_TEST_CASE(SimplePlanWithConstrainedVar) {
  auto device = createTestDeviceFullSize(TEST_TARGET, 1);
  auto &target = device.getTarget();
  poplar::Graph graph(target);

  // Finds plan with a resonable constraint
  popnn::ctc::plan(
      graph, inType, outType, batchSize, maxTime, maxLabelLength, numClasses,
      {{"planConstraints", R"delim({"parallel": {"time": 1}})delim"}});

  // Can't find plan with un-satisfiable constraint
  BOOST_CHECK_THROW(
      popnn::ctc::plan(
          graph, inType, outType, batchSize, maxTime, maxLabelLength,
          numClasses,
          {{"planConstraints", R"delim({"parallel": {"batch": 0}})delim"}}),
      poputil::poplibs_error);

  // Throws on invalid name
  BOOST_CHECK_THROW(
      popnn::ctc::plan(
          graph, inType, outType, batchSize, maxTime, maxLabelLength,
          numClasses,
          {{"planConstraints", R"delim({"parallel": {"xyz": 0}})delim"}}),
      poputil::poplibs_error);
}

BOOST_AUTO_TEST_CASE(WrongPlanType) {
  auto device = createTestDeviceFullSize(TEST_TARGET, 1);
  auto &target = device.getTarget();
  poplar::Graph graph(target);

  // Given an inference plan
  auto invalidPlan = popnn::ctc_infer::plan(graph, poplar::FLOAT, 1, 10, 5, 5);

  // When we call the CTC Loss API, Then we expect exceptions thrown
  BOOST_CHECK_THROW(
      popnn::ctc::createDataInput(graph, poplar::FLOAT, 1, 10, 5, invalidPlan),
      poputil::poplibs_error);
  BOOST_CHECK_THROW(
      popnn::ctc::createLabelsInput(graph, poplar::FLOAT, 1, 10, invalidPlan),
      poputil::poplibs_error);

  poplar::program::Sequence prog{};
  BOOST_CHECK_THROW(
      popnn::ctc::calcLossAndGradientLogProbabilities(
          graph, poplar::FLOAT, {}, {}, {}, {}, prog, 0, invalidPlan),
      poputil::poplibs_error);
  BOOST_CHECK_THROW(popnn::ctc::calcLossAndGradientLogits(graph, poplar::FLOAT,
                                                          {}, {}, {}, {}, prog,
                                                          0, invalidPlan),
                    poputil::poplibs_error);
}


//
// Copyright (C) Wojciech Jarosz <wjarosz@gmail.com>. All rights reserved.
// Use of this source code is governed by a BSD-style license that can
// be found in the LICENSE.txt file.
//

#include "HDRImage.h"
#include "DitherMatrix256.h"    // for dither_matrix256
#include <ImfArray.h>            // for Array2D
#include <ImfRgbaFile.h>         // for RgbaInputFile, RgbaOutputFile
#include <ImathBox.h>            // for Box2i
#include <ImfTestFile.h>         // for isOpenExrFile
#include <ImathVec.h>            // for Vec2
#include <ImfRgba.h>             // for Rgba, RgbaChannels::WRITE_RGBA
#include <ctype.h>               // for tolower
#include <half.h>                // for half
#include <stdlib.h>              // for abs
#include <algorithm>             // for nth_element, transform
#include <cmath>                 // for floor, pow, exp, ceil, round, sqrt
#include <exception>             // for exception
#include <functional>            // for pointer_to_unary_function, function
#include <stdexcept>             // for runtime_error, out_of_range
#include <string>                // for allocator, operator==, basic_string
#include <vector>                // for vector
#include "Common.h"              // for lerp, mod, clamp, getExtension
#include "Colorspace.h"
#include "ParallelFor.h"
#include "Timer.h"
#include <Eigen/Dense>
#include <spdlog/spdlog.h>

// these pragmas ignore warnings about unused static functions
#if defined(__clang__)
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wunused-function"
#elif defined(__GNUC__) || defined(__GNUG__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-function"
#elif defined(_MSC_VER)
#pragma warning (push, 0)
#endif

// since NanoVG includes an old version of stb_image, we declare it static here
#define STB_IMAGE_STATIC
#define STB_IMAGE_IMPLEMENTATION
#include "stb_image.h"
#undef STB_IMAGE_IMPLEMENTATION

#define TINY_DNG_LOADER_IMPLEMENTATION
#include "tiny_dng_loader.h"

#if defined(__clang__)
#pragma clang diagnostic pop
#elif defined(__GNUC__) || defined(__GNUG__)
#pragma GCC diagnostic pop
#elif defined(_MSC_VER)
#pragma warning (pop)
#endif

#define STB_IMAGE_WRITE_IMPLEMENTATION
#include "stb_image_write.h"     // for stbi_write_bmp, stbi_write_hdr, stbi...

#include "PFM.h"
#include "PPM.h"

using namespace Eigen;
using namespace std;

// local functions
namespace
{

inline unsigned short endianSwap(unsigned short val);
void decode12BitToFloat(vector<float> &image, unsigned char *data, int width, int height, bool swapEndian);
void decode14BitToFloat(vector<float> &image, unsigned char *data, int width, int height, bool swapEndian);
void decode16BitToFloat(vector<float> &image, unsigned char *data, int width, int height, bool swapEndian);
void printImageInfo(const tinydng::DNGImage & image);
HDRImage develop(vector<float> & raw,
                 const tinydng::DNGImage & param1,
                 const tinydng::DNGImage & param2);
void copyPixelsFromArray(HDRImage & img, float * data, int w, int h, int n, bool convertToLinear)
{
	if (n != 3 && n != 4)
		throw runtime_error("Only 3- and 4-channel images are supported.");

	// for every pixel in the image
	parallel_for(0, h, [&img,w,n,data,convertToLinear](int y)
	{
		for (int x = 0; x < w; ++x)
		{
			Color4 c(data[n * (x + y * w) + 0],
			         data[n * (x + y * w) + 1],
			         data[n * (x + y * w) + 2],
			         (n == 3) ? 1.f : data[4 * (x + y * w) + 3]);
			img(x, y) = convertToLinear ? SRGBToLinear(c) : c;
		}
	});
}

bool isSTBImage(const string & filename)
{
	FILE *f = stbi__fopen(filename.c_str(), "rb");
	if (!f)
		return false;

	stbi__context s;
	stbi__start_file(&s,f);

	// try stb library first
	if (stbi__jpeg_test(&s) ||
		stbi__png_test(&s) ||
		stbi__bmp_test(&s) ||
		stbi__gif_test(&s) ||
		stbi__psd_test(&s) ||
		stbi__pic_test(&s) ||
		stbi__pnm_test(&s) ||
		stbi__hdr_test(&s) ||
		stbi__tga_test(&s))
	{
		fclose(f);
		return true;
	}

	fclose(f);
	return false;
}

} // namespace

bool HDRImage::load(const string & filename)
{
	auto console = spdlog::get("console");
    string errors;
	string extension = getExtension(filename);
	transform(extension.begin(),
	          extension.end(),
	          extension.begin(),
	          ::tolower);

    int n, w, h;

	// try stb library first
	if (isSTBImage(filename))
	{
		// stbi doesn't do proper srgb, but uses gamma=2.2 instead, so override it.
		// we'll do our own srgb correction
		stbi_ldr_to_hdr_scale(1.0f);
		stbi_ldr_to_hdr_gamma(1.0f);

		float * float_data = stbi_loadf(filename.c_str(), &w, &h, &n, 4);
		if (float_data)
		{
			resize(w, h);
			bool convertToLinear = !stbi_is_hdr(filename.c_str());
			Timer timer;
			copyPixelsFromArray(*this, float_data, w, h, 4, convertToLinear);
			console->debug("Copying image data took: {} seconds.", (timer.elapsed()/1000.f));

			stbi_image_free(float_data);
			return true;
		}
		else
		{
			errors += string("\t") + stbi_failure_reason() + "\n";
		}
	}

    // then try pfm
	if (isPFMImage(filename.c_str()))
    {
	    float * float_data = 0;
	    try
	    {
		    w = 0;
		    h = 0;

		    if ((float_data = loadPFMImage(filename.c_str(), &w, &h, &n)))
		    {
			    if (n == 3)
			    {
				    resize(w, h);

				    Timer timer;
				    // convert 3-channel pfm data to 4-channel internal representation
				    copyPixelsFromArray(*this, float_data, w, h, 3, false);
				    console->debug("Copying image data took: {} seconds.", (timer.elapsed() / 1000.f));

				    delete [] float_data;
				    return true;
			    }
			    else
				    throw runtime_error("Only 3-channel PFMs are currently supported.");
			    return true;
		    }
		    else
			    throw runtime_error("Could not load PFM image.");
	    }
	    catch (const exception &e)
	    {
		    delete [] float_data;
		    resize(0, 0);
		    errors += string("\t") + e.what() + "\n";
	    }
    }

    // next try exrs
	if (Imf::isOpenExrFile(filename.c_str()))
    {
	    try
	    {
		    // FIXME: the threading below seems to cause issues, but shouldn't.
		    // turning off for now
		    Imf::setGlobalThreadCount(thread::hardware_concurrency());
		    Timer timer;

		    Imf::RgbaInputFile file(filename.c_str());
		    Imath::Box2i dw = file.dataWindow();

		    w = dw.max.x - dw.min.x + 1;
		    h = dw.max.y - dw.min.y + 1;

		    Imf::Array2D<Imf::Rgba> pixels(h, w);

		    file.setFrameBuffer(&pixels[0][0] - dw.min.x - dw.min.y * w, 1, w);
		    file.readPixels(dw.min.y, dw.max.y);

		    console->debug("Reading EXR image took: {} seconds.", (timer.lap() / 1000.f));

		    resize(w, h);

		    // copy pixels over to the Image
		    parallel_for(0, h, [this, w, &pixels](int y)
		    {
			    for (int x = 0; x < w; ++x)
			    {
				    const Imf::Rgba &p = pixels[y][x];
				    (*this)(x, y) = Color4(p.r, p.g, p.b, p.a);
			    }
		    });

		    console->debug("Copying EXR image data took: {} seconds.", (timer.lap() / 1000.f));
		    return true;
	    }
	    catch (const exception &e)
	    {
		    resize(0, 0);
		    errors += string("\t") + e.what() + "\n";
	    }
    }

	try
	{
		vector<tinydng::DNGImage> images;
		{
			std::string err;
			vector<tinydng::FieldInfo> customFields;
			bool ret = tinydng::LoadDNG(filename.c_str(), customFields, &images, &err);

			if (ret == false)
				throw runtime_error("Failed to load DNG. " + err);
		}

		// DNG files sometimes only store the orientation in one of the images,
		// instead of all of them. find any set value and save it
		int orientation = 0;
		for (size_t i = 0; i < images.size(); i++)
		{
			console->debug("Image [{}] size = {} x {}.", i, images[i].width, images[i].height);
			console->debug("Image [{}] orientation = {}", i, images[i].orientation);
			if (images[i].orientation != 0)
				orientation = images[i].orientation;
		}

		// Find largest image based on width.
		size_t imageIndex = size_t(-1);
		{
			size_t largest = 0;
			int largestWidth = images[0].width;
			for (size_t i = 0; i < images.size(); i++)
			{
				if (largestWidth < images[i].width)
				{
					largest = i;
					largestWidth = images[i].width;
				}
			}

			imageIndex = largest;
		}
		tinydng::DNGImage & image = images[imageIndex];

		console->debug("\nLargest image within DNG:");
		printImageInfo(image);
		console->debug("\nLast image within DNG:");
		printImageInfo(images.back());

		console->debug("Loading image [{}].", imageIndex);

		w = image.width;
		h = image.height;

		// Convert to float.
		vector<float> hdr;
		bool endianSwap = false;        // TODO

		int spp = image.samples_per_pixel;
		if (image.bits_per_sample == 12)
			decode12BitToFloat(hdr, &(image.data.at(0)), w, h * spp, endianSwap);
		else if (image.bits_per_sample == 14)
			decode14BitToFloat(hdr, &(image.data.at(0)), w, h * spp, endianSwap);
		else if (image.bits_per_sample == 16)
			decode16BitToFloat(hdr, &(image.data.at(0)), w, h * spp, endianSwap);
		else
			throw runtime_error("Error loading DNG: Unsupported bits_per_sample : " + to_string(spp));

		float invScale = 1.0f / static_cast<float>((1 << image.bits_per_sample));
		if (spp == 3)
		{
			console->debug("Decoding a 3 sample-per-pixel DNG image.");
			// normalize
			parallel_for(0, hdr.size(), [&hdr,invScale](int i)
			{
				hdr[i] *= invScale;
			});

			// Create color image & normalize intensity.
			resize(w, h);

			Timer timer;
			// normalize
			parallel_for(0, h, [this,w,invScale,&hdr](int y)
			{
				for (int x = 0; x < w; ++x)
				{
					int index = 3 * y * w + x;
					(*this)(x, y) = Color4(hdr[index] * invScale + 0,
					                       hdr[index] * invScale + 1,
					                       hdr[index] * invScale + 2, 1.0f);
				}
			});
			console->debug("Copying image data took: {} seconds.", (timer.elapsed()/1000.f));
		}
		else if (spp == 1)
		{
			// Create grayscale image & normalize intensity.
			console->debug("Decoding a 1 sample-per-pixel DNG image.");
			Timer timer;
			*this = develop(hdr, image, images.back());
			console->debug("Copying image data took: {} seconds.", (timer.elapsed()/1000.f));
		}
		else
			throw runtime_error("Error loading DNG: Unsupported samples per pixel: " + to_string(spp));

		int startRow = clamp(image.active_area[1], 0, w);
		int endRow = clamp(image.active_area[3], 0, w);
		int startCol = clamp(image.active_area[0], 0, h);
		int endCol = clamp(image.active_area[2], 0, h);

		*this = block(startRow, startCol,
		              endRow-startRow,
		              endCol-startCol).eval();

		enum Orientations
		{
			ORIENTATION_TOPLEFT = 1,
			ORIENTATION_TOPRIGHT = 2,
			ORIENTATION_BOTRIGHT = 3,
			ORIENTATION_BOTLEFT = 4,
			ORIENTATION_LEFTTOP = 5,
			ORIENTATION_RIGHTTOP = 6,
			ORIENTATION_RIGHTBOT = 7,
			ORIENTATION_LEFTBOT = 8
		};

		// now rotate image based on stored orientation
		switch (orientation)
		{
			case ORIENTATION_TOPRIGHT: *this = flippedHorizontal(); break;
			case ORIENTATION_BOTRIGHT: *this = flippedVertical().flippedHorizontal(); break;
			case ORIENTATION_BOTLEFT : *this = flippedVertical(); break;
			case ORIENTATION_LEFTTOP : *this = rotated90CCW().flippedVertical(); break;
			case ORIENTATION_RIGHTTOP: *this = rotated90CW(); break;
			case ORIENTATION_RIGHTBOT: *this = rotated90CW().flippedVertical(); break;
			case ORIENTATION_LEFTBOT : *this = rotated90CCW(); break;
			default: break;// none (0), or ORIENTATION_TOPLEFT
		}

		return true;
	}
	catch (const exception &e)
	{
		resize(0,0);
		// only report errors to the user if the extension was actually dng
		if (extension == "dng")
			errors += string("\t") + e.what() + "\n";
	}

    console->error("ERROR: Unable to read image file \"{}\":\n{}", filename, errors);

    return false;
}

shared_ptr<HDRImage> loadImage(const string & filename)
{
	shared_ptr<HDRImage> ret = make_shared<HDRImage>();
	if (ret->load(filename))
		return ret;
	return nullptr;
}

bool HDRImage::save(const string & filename,
                    float gain, float gamma,
                    bool sRGB, bool dither) const
{
	auto console = spdlog::get("console");
    string extension = getExtension(filename);

    transform(extension.begin(),
              extension.end(),
              extension.begin(),
              ::tolower);

    auto img = this;
    HDRImage imgCopy;

    bool hdrFormat = (extension == "hdr") || (extension == "pfm") || (extension == "exr");

    // if we need to tonemap, then modify a copy of the image data
    if (gain != 1.0f || sRGB || gamma != 1.0f)
    {
        Color4 gainC = Color4(gain, gain, gain, 1.0f);
        Color4 gammaC = Color4(1.0f / gamma, 1.0f / gamma, 1.0f / gamma, 1.0f);

        imgCopy = *this;
        img = &imgCopy;

        if (gain != 1.0f)
            imgCopy *= gainC;

        // only do gamma or sRGB tonemapping if we are saving to an LDR format
        if (!hdrFormat)
        {
            if (sRGB)
                imgCopy = imgCopy.unaryExpr(ptr_fun((Color4 (*)(const Color4 &)) LinearToSRGB));
            else if (gamma != 1.0f)
                imgCopy = imgCopy.pow(gammaC);
        }
    }

    if (extension == "hdr")
        return stbi_write_hdr(filename.c_str(), width(), height(), 4, (const float *) img->data()) != 0;
    else if (extension == "pfm")
        return writePFMImage(filename.c_str(), width(), height(), 4, (const float *) img->data()) != 0;
    else if (extension == "exr")
    {
        try
        {
            Imf::setGlobalThreadCount(thread::hardware_concurrency());
            Imf::RgbaOutputFile file(filename.c_str(), width(), height(), Imf::WRITE_RGBA);
            Imf::Array2D<Imf::Rgba> pixels(height(), width());

            Timer timer;
            // copy image data over to Rgba pixels
            parallel_for(0, height(), [this,img,&pixels](int y)
            {
                for (int x = 0; x < width(); ++x)
                {
                    Imf::Rgba &p = pixels[y][x];
                    Color4 c = (*img)(x, y);
                    p.r = c[0];
                    p.g = c[1];
                    p.b = c[2];
                    p.a = c[3];
                }
            });
            console->debug("Copying pixel data took: {} seconds.", (timer.lap()/1000.f));

            file.setFrameBuffer(&pixels[0][0], 1, width());
            file.writePixels(height());

            console->debug("Writing EXR image took: {} seconds.", (timer.lap()/1000.f));
			return true;
        }
        catch (const exception &e)
        {
            console->error("ERROR: Unable to write image file \"{}\": {}", filename, e.what());
            return false;
        }
    }
    else
    {
        // convert floating-point image to 8-bit per channel with dithering
        vector<unsigned char> data(size()*3, 0);

        Timer timer;
        // convert 3-channel pfm data to 4-channel internal representation
        parallel_for(0, height(), [this,img,&data,dither](int y)
        {
            for (int x = 0; x < width(); ++x)
            {
                Color4 c = (*img)(x, y);
                if (dither)
                {
                    int xmod = x % 256;
                    int ymod = y % 256;
                    float ditherValue = (dither_matrix256[xmod + ymod * 256] / 65536.0f - 0.5f) / 255.0f;
                    c += Color4(Color3(ditherValue), 0.0f);
                }

                // convert to [0-255] range
                c = (c * 255.0f).max(0.0f).min(255.0f);

                data[3 * x + 3 * y * width() + 0] = (unsigned char) c[0];
                data[3 * x + 3 * y * width() + 1] = (unsigned char) c[1];
                data[3 * x + 3 * y * width() + 2] = (unsigned char) c[2];
            }
        });
        console->debug("Tonemapping to 8bit took: {} seconds.", (timer.elapsed()/1000.f));

        if (extension == "ppm")
            return writePPMImage(filename.c_str(), width(), height(), 3, &data[0]);
        else if (extension == "png")
            return stbi_write_png(filename.c_str(), width(), height(),
                                  3, &data[0], sizeof(unsigned char)*width()*3) != 0;
        else if (extension == "bmp")
            return stbi_write_bmp(filename.c_str(), width(), height(), 3, &data[0]) != 0;
        else if (extension == "tga")
            return stbi_write_tga(filename.c_str(), width(), height(), 3, &data[0]) != 0;
        else if (extension == "jpg" || extension == "jpeg")
            return stbi_write_jpg(filename.c_str(), width(), height(), 3, &data[0], 100) != 0;
        else
            throw invalid_argument("Could not determine desired file type from extension.");
    }
}

// local functions
namespace
{

// Taken from http://www.brucelindbloom.com/index.html?Eqn_ChromAdapt.html
const Matrix3f XYZD65TosRGB(
	(Matrix3f() << 3.2406f, -1.5372f, -0.4986f,
		-0.9689f,  1.8758f,  0.0415f,
		0.0557f, -0.2040f,  1.0570f).finished());

const Matrix3f XYZD50ToXYZD65(
	(Matrix3f() << 0.9555766f, -0.0230393f, 0.0631636f,
		-0.0282895f,  1.0099416f, 0.0210077f,
		0.0122982f, -0.0204830f, 1.3299098f).finished());

// Taken from http://www.brucelindbloom.com/index.html?Eqn_RGB_XYZ_Matrix.html
const Matrix3f XYZD50TosRGB(
	(Matrix3f() << 3.2404542f, -1.5371385f, -0.4985314f,
		-0.9692660f,  1.8760108f,  0.0415560f,
		0.0556434f, -0.2040259f,  1.0572252).finished());

Matrix3f computeCameraToXYZD50(const tinydng::DNGImage &param)
{
	//
	// The full DNG color-correction model is described in the
	// "Mapping Camera Color Space to CIE XYZ Space" section of the DNG spec.
	//
	// Let n be the dimensionality of the camera color space (usually 3 or 4).
	// Let CM be the n-by-3 matrix interpolated from the ColorMatrix1 and ColorMatrix2 tags.
	// Let CC be the n-by-n matrix interpolated from the CameraCalibration1 and CameraCalibration2 tags (or identity matrices, if the signatures don't match).
	// Let AB be the n-by-n matrix, which is zero except for the diagonal entries, which are defined by the AnalogBalance tag.
	// Let RM be the 3-by-n matrix interpolated from the ReductionMatrix1 and ReductionMatrix2 tags.
	// Let FM be the 3-by-n matrix interpolated from the ForwardMatrix1 and ForwardMatrix2 tags.

	// TODO: the color correction code below is not quite correct

	// if the ForwardMatrix is included:
	if (false)//param.has_forward_matrix2)
	{
		auto FM((Matrix3f() << param.forward_matrix2[0][0], param.forward_matrix2[0][1], param.forward_matrix2[0][2],
			                   param.forward_matrix2[1][0], param.forward_matrix2[1][1], param.forward_matrix2[1][2],
							   param.forward_matrix2[2][0], param.forward_matrix2[2][1], param.forward_matrix2[2][2]).finished());
		auto CC((Matrix3f() << param.camera_calibration2[0][0], param.camera_calibration2[0][1], param.camera_calibration2[0][2],
							   param.camera_calibration2[1][0], param.camera_calibration2[1][1], param.camera_calibration2[1][2],
							   param.camera_calibration2[2][0], param.camera_calibration2[2][1], param.camera_calibration2[2][2]).finished());
		auto AB = Vector3f(param.analog_balance[0], param.analog_balance[1], param.analog_balance[2]).asDiagonal();

		Vector3f CameraNeutral(param.as_shot_neutral[0],
		                       param.as_shot_neutral[1],
		                       param.as_shot_neutral[2]);
		Vector3f ReferenceNeutral = (AB * CC).inverse() * CameraNeutral;
		auto D = (ReferenceNeutral.asDiagonal()).inverse();
		auto CameraToXYZ = FM * D * (AB * CC).inverse();

		return CameraToXYZ;
	}
	else
	{
		auto CM((Matrix3f() << param.color_matrix2[0][0], param.color_matrix2[0][1], param.color_matrix2[0][2],
			                   param.color_matrix2[1][0], param.color_matrix2[1][1], param.color_matrix2[1][2],
				               param.color_matrix2[2][0], param.color_matrix2[2][1], param.color_matrix2[2][2]).finished());

		auto CameraToXYZ = CM.inverse();

		return CameraToXYZ;

	}
}

HDRImage develop(vector<float> & raw,
                 const tinydng::DNGImage & param1,
                 const tinydng::DNGImage & param2)
{
	Timer timer;

	int width = param1.width;
	int height = param1.height;
	int blackLevel = param1.black_level[0];
	int whiteLevel = param1.white_level[0];
	Vector2i redOffset(param1.active_area[1] % 2, param1.active_area[0] % 2);

	HDRImage developed(width, height);

	Matrix3f CameraToXYZD50 = computeCameraToXYZD50(param2);
	Matrix3f CameraTosRGB = XYZD50TosRGB * CameraToXYZD50;

	// Chapter 5 of DNG spec
	// Map raw values to linear reference values (i.e. adjust for black and white level)
	//
	// we also apply white balance before demosaicing here because it increases the
	// correlation between the color channels and reduces artifacts
	Vector3f wb(param2.as_shot_neutral[0], param2.as_shot_neutral[1], param2.as_shot_neutral[2]);
	const float invScale = 1.0f / (whiteLevel - blackLevel);
	parallel_for(0, developed.height(), [&developed,&raw,blackLevel,invScale,&wb](int y)
	{
		for (int x = 0; x < developed.width(); x++)
		{
			float v = clamp((raw[y * developed.width() + x] - blackLevel)*invScale, 0.f, 1.f);
			Vector3f rgb = Vector3f(v,v,v);
			rgb = rgb.cwiseQuotient(wb);
			developed(x,y) = Color4(rgb(0),rgb(1),rgb(2),1.f);
		}
	});

	// demosaic
//	developed.demosaicLinear(redOffset);
//	developed.demosaicGreenGuidedLinear(redOffset);
//	developed.demosaicMalvar(redOffset);
	developed.demosaicAHD(redOffset, XYZD50ToXYZD65 * CameraToXYZD50);

	// color correction
	// also undo the white balance since the color correction matrix already includes it
	parallel_for(0, developed.height(), [&developed,&CameraTosRGB,&wb](int y)
	{
		for (int x = 0; x < developed.width(); x++)
		{
			Vector3f rgb(developed(x,y).r, developed(x,y).g, developed(x,y).b);
			rgb = rgb.cwiseProduct(wb);
			Vector3f sRGB = CameraTosRGB * rgb;
			developed(x,y) = Color4(sRGB.x(),sRGB.y(),sRGB.z(),1.f);
		}
	});

	spdlog::get("console")->debug("Developing DNG image took {} seconds.", (timer.elapsed()/1000.f));
	return developed;
}

inline unsigned short endianSwap(unsigned short val)
{
	unsigned short ret;

	unsigned char *buf = reinterpret_cast<unsigned char *>(&ret);

	unsigned short x = val;
	buf[1] = static_cast<unsigned char>(x);
	buf[0] = static_cast<unsigned char>(x >> 8);

	return ret;
}

// The decode functions below are adapted from syoyo's dng2exr, in the tinydng library within the
// ext subfolder

//
// Decode 12bit integer image into floating point HDR image
//
void decode12BitToFloat(vector<float> &image, unsigned char *data, int width, int height, bool swapEndian)
{
	Timer timer;

	int offsets[2][2] = {{0, 1}, {1, 2}};
	int bitShifts[2] = {4, 0};

	image.resize(static_cast<size_t>(width * height));

	parallel_for(0, height, [&image,width,&offsets,&bitShifts,data,swapEndian](int y)
	{
		for (int x = 0; x < width; x++)
		{
			unsigned char buf[3];

			// Calculate load address for 12bit pixel(three 8 bit pixels)
			int n = int(y * width + x);

			// 24 = 12bit * 2 pixel, 8bit * 3 pixel
			int n2 = n % 2;           // used for offset & bitshifts
			int addr3 = (n / 2) * 3;  // 8bit pixel pos
			int odd = (addr3 % 2);

			int bit_shift;
			bit_shift = bitShifts[n2];

			int offset[2];
			offset[0] = offsets[n2][0];
			offset[1] = offsets[n2][1];

			if (swapEndian)
			{
				// load with short byte swap
				if (odd)
				{
					buf[0] = data[addr3 - 1];
					buf[1] = data[addr3 + 2];
					buf[2] = data[addr3 + 1];
				}
				else
				{
					buf[0] = data[addr3 + 1];
					buf[1] = data[addr3 + 0];
					buf[2] = data[addr3 + 3];
				}
			}
			else
			{
				buf[0] = data[addr3 + 0];
				buf[1] = data[addr3 + 1];
				buf[2] = data[addr3 + 2];
			}
			unsigned int b0 = static_cast<unsigned int>(buf[offset[0]] & 0xff);
			unsigned int b1 = static_cast<unsigned int>(buf[offset[1]] & 0xff);

			unsigned int val = (b0 << 8) | b1;
			val = 0xfff & (val >> bit_shift);

			image[static_cast<size_t>(y * width + x)] = static_cast<float>(val);
		}
	});

	spdlog::get("console")->debug("decode12BitToFloat took: {} seconds.", (timer.lap() / 1000.f));
}

//
// Decode 14bit integer image into floating point HDR image
//
void decode14BitToFloat(vector<float> &image, unsigned char *data, int width, int height, bool swapEndian)
{
	Timer timer;

	int offsets[4][3] = {{0, 0, 1}, {1, 2, 3}, {3, 4, 5}, {5, 5, 6}};
	int bitShifts[4] = {2, 4, 6, 0};

	image.resize(static_cast<size_t>(width * height));

	parallel_for(0, height, [&image,width,&offsets,&bitShifts,data,swapEndian](int y)
	{
		for (int x = 0; x < width; x++)
		{
			unsigned char buf[7];

			// Calculate load address for 14bit pixel(three 8 bit pixels)
			int n = int(y * width + x);

			// 56 = 14bit * 4 pixel, 8bit * 7 pixel
			int n4 = n % 4;           // used for offset & bitshifts
			int addr7 = (n / 4) * 7;  // 8bit pixel pos
			int odd = (addr7 % 2);

			int offset[3];
			offset[0] = offsets[n4][0];
			offset[1] = offsets[n4][1];
			offset[2] = offsets[n4][2];

			int bit_shift;
			bit_shift = bitShifts[n4];

			if (swapEndian)
			{
				// load with short byte swap
				if (odd)
				{
					buf[0] = data[addr7 - 1];
					buf[1] = data[addr7 + 2];
					buf[2] = data[addr7 + 1];
					buf[3] = data[addr7 + 4];
					buf[4] = data[addr7 + 3];
					buf[5] = data[addr7 + 6];
					buf[6] = data[addr7 + 5];
				}
				else
				{
					buf[0] = data[addr7 + 1];
					buf[1] = data[addr7 + 0];
					buf[2] = data[addr7 + 3];
					buf[3] = data[addr7 + 2];
					buf[4] = data[addr7 + 5];
					buf[5] = data[addr7 + 4];
					buf[6] = data[addr7 + 7];
				}
			}
			else
			{
				memcpy(buf, &data[addr7], 7);
			}
			unsigned int b0 = static_cast<unsigned int>(buf[offset[0]] & 0xff);
			unsigned int b1 = static_cast<unsigned int>(buf[offset[1]] & 0xff);
			unsigned int b2 = static_cast<unsigned int>(buf[offset[2]] & 0xff);

			// unsigned int val = (b0 << 16) | (b1 << 8) | b2;
			// unsigned int val = (b2 << 16) | (b0 << 8) | b0;
			unsigned int val = (b0 << 16) | (b1 << 8) | b2;
			// unsigned int val = b2;
			val = 0x3fff & (val >> bit_shift);

			image[static_cast<size_t>(y * width + x)] = static_cast<float>(val);
		}
	});

	spdlog::get("console")->debug("decode14BitToFloat took: {} seconds.", (timer.lap() / 1000.f));
}

//
// Decode 16bit integer image into floating point HDR image
//
void decode16BitToFloat(vector<float> &image, unsigned char *data, int width, int height, bool swapEndian)
{
	Timer timer;

	image.resize(static_cast<size_t>(width * height));
	unsigned short *ptr = reinterpret_cast<unsigned short *>(data);

	parallel_for(0, height, [&image,width,ptr,swapEndian](int y)
	{
		for (int x = 0; x < width; x++)
		{
			unsigned short val = ptr[y * width + x];
			if (swapEndian)
				val = endianSwap(val);

			// range will be [0, 65535]
			image[static_cast<size_t>(y * width + x)] = static_cast<float>(val);
		}
	});

	spdlog::get("console")->debug("decode16BitToFloat took: {} seconds.", (timer.lap() / 1000.f));
}

char get_colorname(int c)
{
	switch (c)
	{
		case 0:
			return 'R';
		case 1:
			return 'G';
		case 2:
			return 'B';
		case 3:
			return 'C';
		case 4:
			return 'M';
		case 5:
			return 'Y';
		case 6:
			return 'W';
		default:
			return '?';
	}
}

void printImageInfo(const tinydng::DNGImage & image)
{
	auto console = spdlog::get("console");
	console->debug("width = {}.", image.width);
	console->debug("width = {}.", image.width);
	console->debug("height = {}.", image.height);
	console->debug("bits per pixel = {}.", image.bits_per_sample);
	console->debug("bits per pixel(original) = {}", image.bits_per_sample_original);
	console->debug("samples per pixel = {}", image.samples_per_pixel);
	console->debug("sample format = {}", image.sample_format);

	console->debug("version = {}", image.version);

	for (int s = 0; s < image.samples_per_pixel; s++)
	{
		console->debug("white_level[{}] = {}", s, image.white_level[s]);
		console->debug("black_level[{}] = {}", s, image.black_level[s]);
	}

	console->debug("tile_width = {}", image.tile_width);
	console->debug("tile_length = {}", image.tile_length);
	console->debug("tile_offset = {}", image.tile_offset);
	console->debug("tile_offset = {}", image.tile_offset);

	console->debug("cfa_layout = {}", image.cfa_layout);
	console->debug("cfa_plane_color = {}{}{}{}",
	               get_colorname(image.cfa_plane_color[0]),
	               get_colorname(image.cfa_plane_color[1]),
	               get_colorname(image.cfa_plane_color[2]),
	               get_colorname(image.cfa_plane_color[3]));
	console->debug("cfa_pattern[2][2] = \n {}, {},\n {}, {}",
	               image.cfa_pattern[0][0],
	               image.cfa_pattern[0][1],
	               image.cfa_pattern[1][0],
	               image.cfa_pattern[1][1]);

	console->debug("active_area = \n {}, {},\n {}, {}",
	               image.active_area[0],
	               image.active_area[1],
	               image.active_area[2],
	               image.active_area[3]);

	console->debug("calibration_illuminant1 = {}", image.calibration_illuminant1);
	console->debug("calibration_illuminant2 = {}", image.calibration_illuminant2);

	console->debug("color_matrix1 = ");
	for (size_t k = 0; k < 3; k++)
		console->debug("{} {} {}",
		               image.color_matrix1[k][0],
		               image.color_matrix1[k][1],
		               image.color_matrix1[k][2]);

	console->debug("color_matrix2 = ");
	for (size_t k = 0; k < 3; k++)
		console->debug("{} {} {}",
		               image.color_matrix2[k][0],
		               image.color_matrix2[k][1],
		               image.color_matrix2[k][2]);

	if (true)//image.has_forward_matrix2)
	{
		console->debug("forward_matrix1 found = ");
		for (size_t k = 0; k < 3; k++)
			console->debug("{} {} {}",
			               image.forward_matrix1[k][0],
			               image.forward_matrix1[k][1],
			               image.forward_matrix1[k][2]);
	}
	else
		console->debug("forward_matrix2 not found!");

	if (true)//image.has_forward_matrix2)
	{
		console->debug("forward_matrix2 found = ");
		for (size_t k = 0; k < 3; k++)
			console->debug("{} {} {}",
			               image.forward_matrix2[k][0],
			               image.forward_matrix2[k][1],
			               image.forward_matrix2[k][2]);
	}
	else
		console->debug("forward_matrix2 not found!");

	console->debug("camera_calibration1 = ");
	for (size_t k = 0; k < 3; k++)
		console->debug("{} {} {}",
		               image.camera_calibration1[k][0],
		               image.camera_calibration1[k][1],
		               image.camera_calibration1[k][2]);

	console->debug("orientation = {}", image.orientation);

	console->debug("camera_calibration2 = ");
	for (size_t k = 0; k < 3; k++)
		console->debug("{} {} {}",
		               image.camera_calibration2[k][0],
		               image.camera_calibration2[k][1],
		               image.camera_calibration2[k][2]);

	if (image.has_analog_balance)
		console->debug("analog_balance = {} , {} , {}",
		               image.analog_balance[0],
		               image.analog_balance[1],
		               image.analog_balance[2]);
	else
		console->debug("analog_balance not found!");

	if (image.has_as_shot_neutral)
		console->debug("as_shot_neutral = {} , {} , {}",
		               image.as_shot_neutral[0],
		               image.as_shot_neutral[1],
		               image.as_shot_neutral[2]);
	else
		console->debug("shot_neutral not found!");
}

} // namespace


// Copyright 2018 IOTA Foundation

#include <map>
#include <mutex>
#include <random>
#include <thread>
#include <vector>

#include <gtest/gtest.h>

#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm/copy.hpp>

#include "hub/commands/create_user.h"
#include "hub/commands/get_balance.h"
#include "hub/commands/helper.h"
#include "hub/commands/process_transfer_batch.h"
#include "hub/commands/tests/helper.h"

#include "runner.h"

using namespace hub;
using namespace hub::tests;
using namespace sqlpp;

namespace {
static constexpr int64_t USER_BALANCE = 10000;
static constexpr int64_t MAX_TRANSFER_UNIT = 50;
static constexpr uint32_t NUM_USERS = 100;
static constexpr uint32_t NUM_TRANSFERS = 200;

class ProcessTransferBatchTest : public CommandTest {};

void processRandomTransfer(std::map<uint64_t, std::string>& idsToUsers,
                           std::map<uint64_t, int64_t>& idsToBalances,
                           std::shared_ptr<common::ClientSession> session,
                           bool threadSafe = false) {
  static std::mutex m;
  std::random_device rd;   // obtain a random number from hardware
  std::mt19937 eng(rd());  // seed the generator
  std::uniform_int_distribution<> distr(1,
                                        MAX_TRANSFER_UNIT);  // define the range
  std::vector<std::string> users;
  boost::copy(idsToUsers | boost::adaptors::map_values,
              std::inserter(users, users.begin()));
  cmd::ProcessTransferBatch cmd(std::move(session));

  for (uint32_t i = 0; i < NUM_TRANSFERS; ++i) {
    cmd::ProcessTransferBatchRequest req;
    cmd::ProcessTransferBatchReply res;

    auto idsToAmounts = createZigZagTransfer(users, req, distr(eng));
    bool cmdOK = false;
    try {
      auto status = cmd.process(&req, &res);
      cmdOK = (status == common::cmd::OK);
    } catch (const sqlpp::exception& ex) {
    }

    if (threadSafe) {
      if (cmdOK) {
        std::unique_lock<std::mutex> lock(m);
        for (auto& kv : idsToBalances) {
          idsToBalances[kv.first] += idsToAmounts[kv.first];
        }
      }
    } else {
      if (cmdOK) {
        for (auto& kv : idsToBalances) {
          idsToBalances[kv.first] += idsToAmounts[kv.first];
        }
      }
    }
  }
}

TEST_F(ProcessTransferBatchTest, FailOnNonExistingUserId) {
  cmd::ProcessTransferBatchRequest req;
  cmd::ProcessTransferBatchReply res;

  constexpr static auto userId = "Imaginary User";

  req.transfers.emplace_back(
      cmd::UserTransfer{.userId = userId, .amount = 100});
  req.transfers.emplace_back(
      cmd::UserTransfer{.userId = userId, .amount = -100});

  cmd::ProcessTransferBatch command(session());

  auto status = command.process(&req, &res);

  ASSERT_EQ(status, common::cmd::USER_DOES_NOT_EXIST);
}

TEST_F(ProcessTransferBatchTest, ZeroAmountTransferFails) {
  cmd::ProcessTransferBatchRequest req;
  cmd::ProcessTransferBatchReply res;

  auto userId = "User Id";
  auto status = createUser(session(), userId);
  ASSERT_TRUE(status == common::cmd::OK);

  req.transfers.emplace_back(cmd::UserTransfer{.userId = userId, .amount = 0});

  cmd::ProcessTransferBatch command(session());

  status = command.process(&req, &res);
  ASSERT_EQ(status, common::cmd::BATCH_INVALID);
}

TEST_F(ProcessTransferBatchTest, TransfersAreRecorded) {
  cmd::ProcessTransferBatchRequest req;
  cmd::ProcessTransferBatchReply res;
  std::vector<std::string> users;
  std::vector<uint64_t> userIds;
  cmd::ProcessTransferBatch command(session());
  int64_t absAmount = 10;

  for (auto i = 0; i < 10; ++i) {
    users.push_back("User " + std::to_string(i + 1));
    userIds.push_back(i + 1);  // incremental keys are predictable
    auto status = createUser(session(), users[i]);
    ASSERT_TRUE(status == common::cmd::OK);
  }
  createBalanceForUsers(userIds, USER_BALANCE);
  createZigZagTransfer(users, req, absAmount);
  auto status = command.process(&req, &res);

  ASSERT_TRUE(status == common::cmd::OK);

  cmd::GetBalance balCmd(session());
  cmd::GetBalanceRequest getBalReq;
  cmd::GetBalanceReply getBalRep;

  for (uint32_t i = 0; i < users.size(); ++i) {
    getBalReq.userId = users[i];
    int64_t amount = (i % 2) ? absAmount : -absAmount;
    ASSERT_TRUE(balCmd.process(&getBalReq, &getBalRep) == common::cmd::OK);
    ASSERT_EQ(USER_BALANCE + amount, getBalRep.available);
  }
}

TEST_F(ProcessTransferBatchTest, TransfersAreRecordedGroupingUserBalances) {
  cmd::ProcessTransferBatchRequest req;
  cmd::ProcessTransferBatchReply res;
  std::vector<std::string> users;
  std::vector<uint64_t> userIds;
  cmd::ProcessTransferBatch command(session());
  int64_t absAmount = 50;

  for (auto i = 0; i < 10; ++i) {
    users.push_back("User " + std::to_string(i + 1));
    userIds.push_back(i + 1);  // incremental keys are predictable
    auto status = createUser(session(), users[i]);
    ASSERT_TRUE(status == common::cmd::OK);
  }
  createBalanceForUsers(userIds, USER_BALANCE / 2);
  createBalanceForUsers(userIds, USER_BALANCE / 2);
  createZigZagTransfer(users, req, absAmount);
  auto status = command.process(&req, &res);

  ASSERT_TRUE(status == common::cmd::OK);

  cmd::GetBalance balCmd(session());
  cmd::GetBalanceRequest getBalReq;
  cmd::GetBalanceReply getBalRep;

  for (uint32_t i = 0; i < users.size(); ++i) {
    getBalReq.userId = users[i];
    int64_t amount = (i % 2) ? absAmount : -absAmount;
    ASSERT_TRUE(balCmd.process(&getBalReq, &getBalRep) == common::cmd::OK);
    ASSERT_EQ(USER_BALANCE + amount, getBalRep.available);
  }
}

TEST_F(ProcessTransferBatchTest, TransfersMustBeZeroSummed) {
  cmd::ProcessTransferBatchRequest req;
  cmd::ProcessTransferBatchReply res;
  std::vector<std::string> users;
  std::vector<uint64_t> userIds;
  cmd::ProcessTransferBatch command(session());

  for (auto i = 0; i < 9; ++i) {
    users.push_back("User " + std::to_string(i + 1));
    userIds.push_back(i + 1);  // incremental keys are predictable
    auto status = createUser(session(), users[i]);
    ASSERT_TRUE(status == common::cmd::OK);
  }
  createBalanceForUsers(userIds, USER_BALANCE);
  createZigZagTransfer(users, req, USER_BALANCE / 2);
  auto status = command.process(&req, &res);

  ASSERT_FALSE(status == common::cmd::OK);
  ASSERT_EQ(status, common::cmd::BATCH_AMOUNT_NOT_ZERO);
}

TEST_F(ProcessTransferBatchTest, TransferMustHaveSufficientFunds) {
  cmd::ProcessTransferBatchRequest req;
  cmd::ProcessTransferBatchReply res;
  std::vector<std::string> users;
  std::vector<uint64_t> userIds;
  cmd::ProcessTransferBatch command(session());

  for (auto i = 0; i < 2; ++i) {
    users.push_back("User " + std::to_string(i + 1));
    userIds.push_back(i + 1);  // incremental keys are predictable
    auto status = createUser(session(), users[i]);
    ASSERT_TRUE(status == common::cmd::OK);
  }
  createBalanceForUsers(userIds, USER_BALANCE);
  createZigZagTransfer(users, req, USER_BALANCE + 1);
  auto status = command.process(&req, &res);

  ASSERT_EQ(status, common::cmd::BATCH_INCONSISTENT);
}

TEST_F(ProcessTransferBatchTest, SequentialTransfersAreConsistent) {
  std::map<uint64_t, std::string> idsToUsers;
  std::map<uint64_t, int64_t> idsToBalances;

  for (uint32_t i = 0; i < NUM_USERS; ++i) {
    idsToUsers[i + 1] = "User " + std::to_string(i + 1);
    auto status = createUser(session(), idsToUsers[i + 1]);
    ASSERT_EQ(status, common::cmd::OK);
  }

  std::vector<uint64_t> userIds;
  boost::copy(idsToUsers | boost::adaptors::map_keys,
              std::inserter(userIds, userIds.begin()));

  idsToBalances = createBalanceForUsers(userIds, USER_BALANCE);

  for (auto i = 0; i < 10; ++i) {
    processRandomTransfer(idsToUsers, idsToBalances, session());
  }

  cmd::GetBalance balCmd(session());
  cmd::GetBalanceRequest getBalReq;
  cmd::GetBalanceReply getBalRep;

  for (auto& kv : idsToUsers) {
    getBalReq.userId = kv.second;
    ASSERT_EQ(balCmd.process(&getBalReq, &getBalRep), common::cmd::OK);
    ASSERT_EQ(idsToBalances[kv.first], getBalRep.available);
  }
}

TEST_F(ProcessTransferBatchTest, ConcurrentTransfersAreConsistent) {
  static constexpr uint32_t NUM_THREADS = 20;

  std::vector<std::thread> threads(NUM_THREADS);
  std::map<uint64_t, std::string> idsToUsers;
  std::map<uint64_t, int64_t> idsToBalances;

  for (uint32_t i = 0; i < NUM_USERS; ++i) {
    idsToUsers[i + 1] = "User " + std::to_string(i + 1);
    auto status = createUser(session(), idsToUsers[i + 1]);
    ASSERT_EQ(status, common::cmd::OK);
  }

  std::vector<uint64_t> userIds;
  boost::copy(idsToUsers | boost::adaptors::map_keys,
              std::inserter(userIds, userIds.begin()));
  idsToBalances = createBalanceForUsers(userIds, USER_BALANCE);

  for (uint32_t i = 0; i < NUM_THREADS; ++i) {
    std::thread t(processRandomTransfer, std::ref(idsToUsers),
                  std::ref(idsToBalances), session(), true);
    threads[i] = std::move(t);
  }

  for (auto& t : threads) {
    t.join();
  }

  cmd::GetBalance balCmd(session());
  cmd::GetBalanceRequest getBalReq;
  cmd::GetBalanceReply getBalRep;

  for (auto& kv : idsToUsers) {
    getBalReq.userId = kv.second;
    ASSERT_EQ(balCmd.process(&getBalReq, &getBalRep), common::cmd::OK);
    ASSERT_EQ(idsToBalances[kv.first], getBalRep.available);
  }
}

};  // namespace