//***************************************************************************************
// HW08App.cpp by Frank Luna (C) 2015 All Rights Reserved.
//***************************************************************************************

#include "../common/d3dApp.h"
#include "../common/MathHelper.h"
#include <UDX12/UploadBuffer.h>
#include "../common/GeometryGenerator.h"
#include "util.h"

#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include <Eigen/Dense>

using Microsoft::WRL::ComPtr;
using namespace DirectX;
using namespace DirectX::PackedVector;

const int gNumFrameResources = 3;
const size_t InterpNum = 60;

struct V;
struct F;
struct H;
using Traits_VFH = Ubpa::HEMeshTriats_EmptyE<V, F, H>;

struct V : Ubpa::TVertex<Traits_VFH> {
	Ubpa::pointf2 p; // t = 0
	Ubpa::pointf2 q; // t = 1
};

struct F : Ubpa::TPolygon<Traits_VFH> {
	float area;

	// set L, theta, S
	void Init();
	Ubpa::matf2 L;
	float theta; // -pi ~ pi
	Ubpa::matf2 S;

	// theta, S, t -> Lt
	void UpdateLt(float t);
	Ubpa::matf2 Lt;
};

struct H : Ubpa::THalfEdge<Traits_VFH> {
	float cot_theta{ 0.f }; // cotangent of opposite angle
};

void F::Init() {
	auto he01 = HalfEdge();
	auto he12 = he01->Next();
	auto he20 = he12->Next();

	auto v0 = he01->Origin();
	auto v1 = he12->Origin();
	auto v2 = he20->Origin();

	const auto& p0 = v0->p;
	const auto& p1 = v1->p;
	const auto& p2 = v2->p;

	const auto& q0 = v0->q;
	const auto& q1 = v1->q;
	const auto& q2 = v2->q;

	float y1_y2 = p1[1] - p2[1];
	float y2_y0 = p2[1] - p0[1];
	float y0_y1 = p0[1] - p1[1];

	float x2_x1 = p2[0] - p1[0];
	float x0_x2 = p0[0] - p2[0];
	float x1_x0 = p1[0] - p0[0];

	Ubpa::matf2 J;
	float inv_two_area = 1 / (2 * area);
	J(0, 0) = inv_two_area * (y1_y2 * q0[0] + y2_y0 * q1[0] + y0_y1 * q2[0]);
	J(0, 1) = inv_two_area * (x2_x1 * q0[0] + x0_x2 * q1[0] + x1_x0 * q2[0]);
	J(1, 0) = inv_two_area * (y1_y2 * q0[1] + y2_y0 * q1[1] + y0_y1 * q2[1]);
	J(1, 1) = inv_two_area * (x2_x1 * q0[1] + x0_x2 * q1[1] + x1_x0 * q2[1]);

	auto [U, S, V] = J.SVD();
	Ubpa::matf2 R = U * V.transpose();
	assert(R.det() > 0);
	this->S = V * S * V.transpose();
	L = R * S;
	float cos_theta = R(0, 0);
	float sin_theta = R(1, 0);
	float ac = std::acos(cos_theta);
	if (sin_theta > 0)
		theta = ac;
	else
		theta = - ac;
}

void F::UpdateLt(float t) {
	assert(0 <= t && t <= 1);
	float theta_t = t * theta;
	float cos_theta_t = std::cos(theta_t);
	float sin_theta_t = std::sin(theta_t);
	Ubpa::matf2 Rt = {
		cos_theta_t, -sin_theta_t,
		sin_theta_t,  cos_theta_t
	};
	Ubpa::matf2 St = Ubpa::matf2::lerp(Ubpa::matf2::eye(), S, t);
	Lt = Rt * St;
}

struct ObjectConstants
{
	DirectX::XMFLOAT4X4 World = MathHelper::Identity4x4();
	DirectX::XMFLOAT4X4 TexTransform = MathHelper::Identity4x4();
};

struct PassConstants
{
	DirectX::XMFLOAT4X4 View = MathHelper::Identity4x4();
	DirectX::XMFLOAT4X4 InvView = MathHelper::Identity4x4();
	DirectX::XMFLOAT4X4 Proj = MathHelper::Identity4x4();
	DirectX::XMFLOAT4X4 InvProj = MathHelper::Identity4x4();
	DirectX::XMFLOAT4X4 ViewProj = MathHelper::Identity4x4();
	DirectX::XMFLOAT4X4 InvViewProj = MathHelper::Identity4x4();
	DirectX::XMFLOAT3 EyePosW = { 0.0f, 0.0f, 0.0f };
	float cbPerObjectPad1 = 0.0f;
	DirectX::XMFLOAT2 RenderTargetSize = { 0.0f, 0.0f };
	DirectX::XMFLOAT2 InvRenderTargetSize = { 0.0f, 0.0f };
	float NearZ = 0.0f;
	float FarZ = 0.0f;
	float TotalTime = 0.0f;
	float DeltaTime = 0.0f;

	DirectX::XMFLOAT4 AmbientLight = { 0.0f, 0.0f, 0.0f, 1.0f };

	// Indices [0, NUM_DIR_LIGHTS) are directional lights;
	// indices [NUM_DIR_LIGHTS, NUM_DIR_LIGHTS+NUM_POINT_LIGHTS) are point lights;
	// indices [NUM_DIR_LIGHTS+NUM_POINT_LIGHTS, NUM_DIR_LIGHTS+NUM_POINT_LIGHT+NUM_SPOT_LIGHTS)
	// are spot lights for a maximum of MaxLights per object.
	Light Lights[MaxLights];
};

struct Vertex
{
	DirectX::XMFLOAT3 Pos;
	DirectX::XMFLOAT3 Normal;
	DirectX::XMFLOAT2 TexC;
};

// Lightweight structure stores parameters to draw a shape.  This will
// vary from app-to-app.
struct RenderItem
{
	RenderItem() = default;

    // World matrix of the shape that describes the object's local space
    // relative to the world space, which defines the position, orientation,
    // and scale of the object in the world.
    XMFLOAT4X4 World = MathHelper::Identity4x4();

	XMFLOAT4X4 TexTransform = MathHelper::Identity4x4();

	// Dirty flag indicating the object data has changed and we need to update the constant buffer.
	// Because we have an object cbuffer for each FrameResource, we have to apply the
	// update to each FrameResource.  Thus, when we modify obect data we should set
	// NumFramesDirty = gNumFrameResources so that each frame resource gets the update.
	int NumFramesDirty = gNumFrameResources;

	// Index into GPU constant buffer corresponding to the ObjectCB for this render item.
	UINT ObjCBIndex = -1;

	Material* Mat = nullptr;
	Ubpa::DX12::MeshGeometry* Geo = nullptr;
	//std::string Geo;

    // Primitive topology.
    D3D12_PRIMITIVE_TOPOLOGY PrimitiveType = D3D11_PRIMITIVE_TOPOLOGY_TRIANGLELIST;

    // DrawIndexedInstanced parameters.
    UINT IndexCount = 0;
    UINT StartIndexLocation = 0;
    int BaseVertexLocation = 0;
};

class HW08App : public D3DApp
{
public:
    HW08App(HINSTANCE hInstance);
    HW08App(const HW08App& rhs) = delete;
    HW08App& operator=(const HW08App& rhs) = delete;
    ~HW08App();

    virtual bool Initialize()override;

private:
    virtual void OnResize()override;
    virtual void Update(const GameTimer& gt)override;
    virtual void Draw(const GameTimer& gt)override;

    virtual void OnMouseDown(WPARAM btnState, int x, int y)override;
    virtual void OnMouseUp(WPARAM btnState, int x, int y)override;
    virtual void OnMouseMove(WPARAM btnState, int x, int y)override;

    void OnKeyboardInput(const GameTimer& gt);
	void UpdateCamera(const GameTimer& gt);
	void AnimateMaterials(const GameTimer& gt);
	void UpdateObjectCBs(const GameTimer& gt);
	void UpdateMaterialCBs(const GameTimer& gt);
	void UpdateMainPassCB(const GameTimer& gt);

	void LoadTextures();
    void BuildRootSignature();
	void BuildDescriptorHeaps();
    void BuildShadersAndInputLayout();
    void BuildShapeGeometry();
    void BuildPSOs();
    void BuildFrameResources();
    void BuildMaterials();
    void BuildRenderItems();
    void DrawRenderItems(ID3D12GraphicsCommandList* cmdList, const std::vector<RenderItem*>& ritems);

	std::array<const CD3DX12_STATIC_SAMPLER_DESC, 6> GetStaticSamplers();

private:

	std::vector<std::unique_ptr<Ubpa::DX12::FrameResource>> mFrameResources;
	Ubpa::DX12::FrameResource* mCurrFrameResource = nullptr;
    int mCurrFrameResourceIndex = 0;

	std::unordered_map<std::string, std::unique_ptr<Material>> mMaterials;

    std::vector<D3D12_INPUT_ELEMENT_DESC> mInputLayout;

	// List of all the render items.
	std::vector<std::unique_ptr<RenderItem>> mAllRitems;

	// Render items divided by PSO.
	std::vector<RenderItem*> mOpaqueRitems;

    PassConstants mMainPassCB;

	XMFLOAT3 mEyePos = { 0.0f, 0.0f, 0.0f };
	XMFLOAT4X4 mView = MathHelper::Identity4x4();
	XMFLOAT4X4 mProj = MathHelper::Identity4x4();

	float mTheta = 1.3f*XM_PI;
	float mPhi = 0.4f*XM_PI;
	float mRadius = 2.5f;

    POINT mLastMousePos;

	std::unordered_map<std::string, std::unique_ptr<Ubpa::TriMesh>> trimeshes;

	// frame graph
	//Ubpa::DX12::FG::RsrcMngr fgRsrcMngr;
	Ubpa::DX12::FG::Executor fgExecutor;
	Ubpa::FG::Compiler fgCompiler;
	Ubpa::FG::FrameGraph fg;
};

int WINAPI WinMain(HINSTANCE hInstance, HINSTANCE prevInstance,
    PSTR cmdLine, int showCmd)
{
    // Enable run-time memory check for debug builds.
#if defined(DEBUG) | defined(_DEBUG)
    _CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);
#endif

    try
    {
        HW08App theApp(hInstance);
        if(!theApp.Initialize())
            return 0;

        int rst = theApp.Run();
		Ubpa::DXRenderer::Instance().Release();
		return rst;
    }
    catch(Ubpa::DX12::Util::Exception& e)
    {
        MessageBox(nullptr, e.ToString().c_str(), L"HR Failed", MB_OK);
        return 0;
    }

}

HW08App::HW08App(HINSTANCE hInstance)
    : D3DApp(hInstance)
{
}

HW08App::~HW08App()
{
    if(!uDevice.IsNull())
        FlushCommandQueue();
}

bool HW08App::Initialize()
{
    if(!D3DApp::Initialize())
        return false;

	Ubpa::DXRenderer::Instance().Init(uDevice.raw.Get());

	Ubpa::DX12::DescriptorHeapMngr::Instance().Init(uDevice.raw.Get(), 1024, 1024, 1024, 1024, 1024);

	//fgRsrcMngr.Init(uGCmdList, uDevice);

    // Reset the command list to prep for initialization commands.
    ThrowIfFailed(uGCmdList->Reset(mDirectCmdListAlloc.Get(), nullptr));

    // Get the increment size of a descriptor in this heap type.  This is hardware specific,
	// so we have to query this information.
    //mCbvSrvDescriptorSize = uDevice->GetDescriptorHandleIncrementSize(D3D12_DESCRIPTOR_HEAP_TYPE_CBV_SRV_UAV);

	Ubpa::DXRenderer::Instance().GetUpload().Begin();

	LoadTextures();
    BuildRootSignature();
	BuildDescriptorHeaps();
    BuildShadersAndInputLayout();
    BuildShapeGeometry();
	BuildMaterials();
    BuildRenderItems();
    BuildFrameResources();
    BuildPSOs();

    // Execute the initialization commands.
    ThrowIfFailed(uGCmdList->Close());
	uCmdQueue.Execute(uGCmdList.raw.Get());

	Ubpa::DXRenderer::Instance().GetUpload().End(uCmdQueue.raw.Get());

    // Wait until initialization is complete.
    FlushCommandQueue();

    return true;
}

void HW08App::OnResize()
{
    D3DApp::OnResize();

    // The window resized, so update the aspect ratio and recompute the projection matrix.
    XMMATRIX P = XMMatrixPerspectiveFovLH(0.25f*MathHelper::Pi, AspectRatio(), 1.0f, 1000.0f);
    XMStoreFloat4x4(&mProj, P);

	auto clearFGRsrcMngr = [](void* rsrcMngr) {
		reinterpret_cast<Ubpa::DX12::FG::RsrcMngr*>(rsrcMngr)->Clear();
	};
	for (auto& frsrc : mFrameResources)
		frsrc->DelayUpdateResource("FrameGraphRsrcMngr", clearFGRsrcMngr);
}

void HW08App::Update(const GameTimer& gt)
{
    OnKeyboardInput(gt);
	UpdateCamera(gt);

    // Cycle through the circular frame resource array.
    mCurrFrameResourceIndex = (mCurrFrameResourceIndex + 1) % gNumFrameResources;
    mCurrFrameResource = mFrameResources[mCurrFrameResourceIndex].get();

    // Has the GPU finished processing the commands of the current frame resource?
    // If not, wait until the GPU has completed commands up to this fence point.
	mCurrFrameResource->Wait();

	AnimateMaterials(gt);
	UpdateObjectCBs(gt);
	UpdateMaterialCBs(gt);
	UpdateMainPassCB(gt);
}

void HW08App::Draw(const GameTimer& gt)
{
	auto cmdListAlloc = mCurrFrameResource->GetResource<ID3D12CommandAllocator>("CommandAllocator");

    // Reuse the memory associated with command recording.
    // We can only reset when the associated command lists have finished execution on the GPU.
    ThrowIfFailed(cmdListAlloc->Reset());

    // A command list can be reset after it has been added to the command queue via ExecuteCommandList.
    // Reusing the command list reuses memory.
	ThrowIfFailed(uGCmdList->Reset(cmdListAlloc, nullptr));
	uGCmdList.SetDescriptorHeaps(Ubpa::DX12::DescriptorHeapMngr::Instance().GetCSUGpuDH()->GetDescriptorHeap());

	uGCmdList->RSSetViewports(1, &mScreenViewport);
	uGCmdList->RSSetScissorRects(1, &mScissorRect);

	fg.Clear();
	auto fgRsrcMngr = mCurrFrameResource->GetResource<Ubpa::DX12::FG::RsrcMngr>("FrameGraphRsrcMngr");
	fgRsrcMngr->NewFrame();
	fgExecutor.NewFrame();;

	auto gbuffer0 = fg.AddResourceNode("GBuffer0");
	auto gbuffer1 = fg.AddResourceNode("GBuffer1");
	auto gbuffer2 = fg.AddResourceNode("GBuffer2");
	auto backbuffer = fg.AddResourceNode("Back Buffer");
	auto depthstencil = fg.AddResourceNode("Depth Stencil");
	auto gbPass = fg.AddPassNode(
		"GBuffer Pass",
		{},
		{ gbuffer0,gbuffer1,gbuffer2,depthstencil }
	);
	/*auto debugPass = fg.AddPassNode(
		"Debug",
		{ gbuffer1 },
		{ backbuffer }
	);*/
	auto deferLightingPass = fg.AddPassNode(
		"Defer Lighting",
		{ gbuffer0,gbuffer1,gbuffer2 },
		{ backbuffer }
	);

	(*fgRsrcMngr)
		.RegisterTemporalRsrc(gbuffer0,
			Ubpa::DX12::FG::RsrcType::RT2D(DXGI_FORMAT_R32G32B32A32_FLOAT, mClientWidth, mClientHeight, Colors::Black))
		.RegisterTemporalRsrc(gbuffer1,
			Ubpa::DX12::FG::RsrcType::RT2D(DXGI_FORMAT_R32G32B32A32_FLOAT, mClientWidth, mClientHeight, Colors::Black))
		.RegisterTemporalRsrc(gbuffer2,
			Ubpa::DX12::FG::RsrcType::RT2D(DXGI_FORMAT_R32G32B32A32_FLOAT, mClientWidth, mClientHeight, Colors::Black))

		.RegisterRsrcTable({
			{gbuffer0,Ubpa::DX12::Desc::SRV::Tex2D(DXGI_FORMAT_R32G32B32A32_FLOAT)},
			{gbuffer1,Ubpa::DX12::Desc::SRV::Tex2D(DXGI_FORMAT_R32G32B32A32_FLOAT)},
			{gbuffer2,Ubpa::DX12::Desc::SRV::Tex2D(DXGI_FORMAT_R32G32B32A32_FLOAT)} })

		.RegisterImportedRsrc(backbuffer, { CurrentBackBuffer(), D3D12_RESOURCE_STATE_PRESENT })
		.RegisterImportedRsrc(depthstencil, { mDepthStencilBuffer.Get(), D3D12_RESOURCE_STATE_DEPTH_WRITE })

		.RegisterPassRsrcs(gbPass, gbuffer0, D3D12_RESOURCE_STATE_RENDER_TARGET,
			Ubpa::DX12::FG::RsrcImplDesc_RTV_Null{})
		.RegisterPassRsrcs(gbPass, gbuffer1, D3D12_RESOURCE_STATE_RENDER_TARGET,
			Ubpa::DX12::FG::RsrcImplDesc_RTV_Null{})
		.RegisterPassRsrcs(gbPass, gbuffer2, D3D12_RESOURCE_STATE_RENDER_TARGET,
			Ubpa::DX12::FG::RsrcImplDesc_RTV_Null{})
		.RegisterPassRsrcs(gbPass, depthstencil,
			D3D12_RESOURCE_STATE_DEPTH_WRITE, Ubpa::DX12::Desc::DSV::Basic(mDepthStencilFormat))

		/*.RegisterPassRsrcs(debugPass, gbuffer1, D3D12_RESOURCE_STATE_PIXEL_SHADER_RESOURCE,
			Ubpa::DX12::Desc::SRV::Tex2D(DXGI_FORMAT_R32G32B32A32_FLOAT))

		.RegisterPassRsrcs(debugPass, backbuffer, D3D12_RESOURCE_STATE_RENDER_TARGET,
			Ubpa::DX12::FG::RsrcImplDesc_RTV_Null{})*/

		.RegisterPassRsrcs(deferLightingPass, gbuffer0, D3D12_RESOURCE_STATE_PIXEL_SHADER_RESOURCE,
			Ubpa::DX12::Desc::SRV::Tex2D(DXGI_FORMAT_R32G32B32A32_FLOAT))
		.RegisterPassRsrcs(deferLightingPass, gbuffer1, D3D12_RESOURCE_STATE_PIXEL_SHADER_RESOURCE,
			Ubpa::DX12::Desc::SRV::Tex2D(DXGI_FORMAT_R32G32B32A32_FLOAT))
		.RegisterPassRsrcs(deferLightingPass, gbuffer2, D3D12_RESOURCE_STATE_PIXEL_SHADER_RESOURCE,
			Ubpa::DX12::Desc::SRV::Tex2D(DXGI_FORMAT_R32G32B32A32_FLOAT))

		.RegisterPassRsrcs(deferLightingPass, backbuffer, D3D12_RESOURCE_STATE_RENDER_TARGET,
			Ubpa::DX12::FG::RsrcImplDesc_RTV_Null{})
		;

	fgExecutor.RegisterPassFunc(
		gbPass,
		[&](const Ubpa::DX12::FG::PassRsrcs& rsrcs) {
			uGCmdList->SetPipelineState(Ubpa::DXRenderer::Instance().GetPSO("geometry"));
			auto gb0 = rsrcs.find(gbuffer0)->second;
			auto gb1 = rsrcs.find(gbuffer1)->second;
			auto gb2 = rsrcs.find(gbuffer2)->second;
			auto ds = rsrcs.find(depthstencil)->second;

			// Clear the render texture and depth buffer.
			uGCmdList.ClearRenderTargetView(gb0.cpuHandle, Colors::Black);
			uGCmdList.ClearRenderTargetView(gb1.cpuHandle, Colors::Black);
			uGCmdList.ClearRenderTargetView(gb2.cpuHandle, Colors::Black);
			uGCmdList.ClearDepthStencilView(ds.cpuHandle);

			// Specify the buffers we are going to render to.
			std::array rts{ gb0.cpuHandle,gb1.cpuHandle,gb2.cpuHandle };
			uGCmdList->OMSetRenderTargets(rts.size(), rts.data(), false, &ds.cpuHandle);

			uGCmdList->SetGraphicsRootSignature(Ubpa::DXRenderer::Instance().GetRootSignature("geometry"));

			auto passCB = mCurrFrameResource
				->GetResource<Ubpa::DX12::ArrayUploadBuffer<PassConstants>>("gbPass constants")
				->GetResource();
			uGCmdList->SetGraphicsRootConstantBufferView(2, passCB->GetGPUVirtualAddress());

			auto fence2interpidx = [N = InterpNum](size_t n, size_t gap) {
				size_t loop = 2 * N * gap;
				size_t i = (n % loop) / gap;
				if (i >= N)
					i = 2 * N - 1 - i;
				return 2 + i;
			};
			DrawRenderItems(uGCmdList.raw.Get(), { mAllRitems[0].get(), mAllRitems[1].get(), mAllRitems[fence2interpidx(mCurrentFence, 2)].get() });
		}
	);

	//fgExecutor.RegisterPassFunc(
	//	debugPass,
	//	[&](const Ubpa::DX12::FG::PassRsrcs& rsrcs) {
	//		uGCmdList->SetPipelineState(Ubpa::DXRenderer::Instance().GetPSO("screen"));
	//		auto img = rsrcs.find(gbuffer1)->second;
	//		auto bb = rsrcs.find(backbuffer)->second;
	//
	//		//uGCmdList->CopyResource(bb.resource, rt.resource);

	//		// Clear the render texture and depth buffer.
	//		uGCmdList.ClearRenderTargetView(bb.cpuHandle, Colors::LightSteelBlue);

	//		// Specify the buffers we are going to render to.
	//		//uGCmdList.OMSetRenderTarget(bb.cpuHandle, ds.cpuHandle);
	//		uGCmdList->OMSetRenderTargets(1, &bb.cpuHandle, false, nullptr);

	//		uGCmdList->SetGraphicsRootSignature(Ubpa::DXRenderer::Instance().GetRootSignature("screen"));

	//		uGCmdList->SetGraphicsRootDescriptorTable(0, img.gpuHandle);

	//		uGCmdList->IASetVertexBuffers(0, 0, nullptr);
	//		uGCmdList->IASetIndexBuffer(nullptr);
	//		uGCmdList->IASetPrimitiveTopology(D3D_PRIMITIVE_TOPOLOGY_TRIANGLELIST);
	//		uGCmdList->DrawInstanced(6, 1, 0, 0);
	//	}
	//);

	fgExecutor.RegisterPassFunc(
		deferLightingPass,
		[&](const Ubpa::DX12::FG::PassRsrcs& rsrcs) {
			uGCmdList->SetPipelineState(Ubpa::DXRenderer::Instance().GetPSO("defer lighting"));
			auto gb0 = rsrcs.find(gbuffer0)->second;
			auto gb1 = rsrcs.find(gbuffer1)->second;
			auto gb2 = rsrcs.find(gbuffer2)->second;

			auto bb = rsrcs.find(backbuffer)->second;

			//uGCmdList->CopyResource(bb.resource, rt.resource);

			// Clear the render texture and depth buffer.
			uGCmdList.ClearRenderTargetView(bb.cpuHandle, Colors::LightSteelBlue);

			// Specify the buffers we are going to render to.
			//uGCmdList.OMSetRenderTarget(bb.cpuHandle, ds.cpuHandle);
			uGCmdList->OMSetRenderTargets(1, &bb.cpuHandle, false, nullptr);

			uGCmdList->SetGraphicsRootSignature(Ubpa::DXRenderer::Instance().GetRootSignature("defer lighting"));

			uGCmdList->SetGraphicsRootDescriptorTable(0, gb0.gpuHandle);

			uGCmdList->IASetVertexBuffers(0, 0, nullptr);
			uGCmdList->IASetIndexBuffer(nullptr);
			uGCmdList->IASetPrimitiveTopology(D3D_PRIMITIVE_TOPOLOGY_TRIANGLELIST);
			uGCmdList->DrawInstanced(6, 1, 0, 0);
		}
	);

	static bool flag{ false };
	if (!flag) {
		OutputDebugStringA(fg.ToGraphvizGraph().Dump().c_str());
		flag = true;
	}

	auto [success, crst] = fgCompiler.Compile(fg);
	fgExecutor.Execute(crst, *fgRsrcMngr);

    // Done recording commands.
    ThrowIfFailed(uGCmdList->Close());

    // Add the command list to the queue for execution.
	uCmdQueue.Execute(uGCmdList.raw.Get());

    // Swap the back and front buffers
    ThrowIfFailed(mSwapChain->Present(0, 0));
	mCurrBackBuffer = (mCurrBackBuffer + 1) % SwapChainBufferCount;

	mCurrFrameResource->Signal(uCmdQueue.raw.Get(), ++mCurrentFence);
}

void HW08App::OnMouseDown(WPARAM btnState, int x, int y)
{
    mLastMousePos.x = x;
    mLastMousePos.y = y;

    SetCapture(mhMainWnd);
}

void HW08App::OnMouseUp(WPARAM btnState, int x, int y)
{
    ReleaseCapture();
}

void HW08App::OnMouseMove(WPARAM btnState, int x, int y)
{
    if((btnState & MK_LBUTTON) != 0)
    {
        // Make each pixel correspond to a quarter of a degree.
        float dx = XMConvertToRadians(0.25f*static_cast<float>(x - mLastMousePos.x));
        float dy = XMConvertToRadians(0.25f*static_cast<float>(y - mLastMousePos.y));

        // Update angles based on input to orbit camera around box.
        mTheta += dx;
        mPhi += dy;

        // Restrict the angle mPhi.
        mPhi = MathHelper::Clamp(mPhi, 0.1f, MathHelper::Pi - 0.1f);
    }
    else if((btnState & MK_RBUTTON) != 0)
    {
        // Make each pixel correspond to 0.2 unit in the scene.
        float dx = 0.05f*static_cast<float>(x - mLastMousePos.x);
        float dy = 0.05f*static_cast<float>(y - mLastMousePos.y);

        // Update the camera radius based on input.
        mRadius += dx - dy;

        // Restrict the radius.
        mRadius = MathHelper::Clamp(mRadius, 2.0f, 150.0f);
    }

    mLastMousePos.x = x;
    mLastMousePos.y = y;
}

void HW08App::OnKeyboardInput(const GameTimer& gt)
{
}

void HW08App::UpdateCamera(const GameTimer& gt)
{
	// Convert Spherical to Cartesian coordinates.
	mEyePos.x = mRadius*sinf(mPhi)*cosf(mTheta);
	mEyePos.z = mRadius*sinf(mPhi)*sinf(mTheta);
	mEyePos.y = mRadius*cosf(mPhi);

	// Build the view matrix.
	XMVECTOR pos = XMVectorSet(mEyePos.x, mEyePos.y, mEyePos.z, 1.0f);
	XMVECTOR target = XMVectorZero();
	XMVECTOR up = XMVectorSet(0.0f, 1.0f, 0.0f, 0.0f);

	XMMATRIX view = XMMatrixLookAtLH(pos, target, up);
	XMStoreFloat4x4(&mView, view);
}

void HW08App::AnimateMaterials(const GameTimer& gt)
{

}

void HW08App::UpdateObjectCBs(const GameTimer& gt)
{
	auto currObjectCB = mCurrFrameResource
		->GetResource<Ubpa::DX12::ArrayUploadBuffer<ObjectConstants>>("ArrayUploadBuffer<ObjectConstants>");
	for(auto& e : mAllRitems)
	{
		// Only update the cbuffer data if the constants have changed.
		// This needs to be tracked per frame resource.
		if(e->NumFramesDirty > 0)
		{
			XMMATRIX world = XMLoadFloat4x4(&e->World);
			XMMATRIX texTransform = XMLoadFloat4x4(&e->TexTransform);

			ObjectConstants objConstants;
			XMStoreFloat4x4(&objConstants.World, XMMatrixTranspose(world));
			XMStoreFloat4x4(&objConstants.TexTransform, XMMatrixTranspose(texTransform));

			currObjectCB->Set(e->ObjCBIndex, objConstants);

			// Next FrameResource need to be updated too.
			e->NumFramesDirty--;
		}
	}
}

void HW08App::UpdateMaterialCBs(const GameTimer& gt)
{
	auto currMaterialCB = mCurrFrameResource
		->GetResource<Ubpa::DX12::ArrayUploadBuffer<MaterialConstants>>("ArrayUploadBuffer<MaterialConstants>");
	for(auto& e : mMaterials)
	{
		// Only update the cbuffer data if the constants have changed.  If the cbuffer
		// data changes, it needs to be updated for each FrameResource.
		Material* mat = e.second.get();
		if(mat->NumFramesDirty > 0)
		{
			XMMATRIX matTransform = XMLoadFloat4x4(&mat->MatTransform);

			MaterialConstants matConstants;
			matConstants.DiffuseAlbedo = mat->DiffuseAlbedo;
			matConstants.FresnelR0 = mat->FresnelR0;
			matConstants.Roughness = mat->Roughness;
			XMStoreFloat4x4(&matConstants.MatTransform, XMMatrixTranspose(matTransform));

			currMaterialCB->Set(mat->MatCBIndex, matConstants);

			// Next FrameResource need to be updated too.
			mat->NumFramesDirty--;
		}
	}
}

void HW08App::UpdateMainPassCB(const GameTimer& gt)
{
	XMMATRIX view = XMLoadFloat4x4(&mView);
	XMMATRIX proj = XMLoadFloat4x4(&mProj);

	XMMATRIX viewProj = XMMatrixMultiply(view, proj);
	XMMATRIX invView = XMMatrixInverse(&XMMatrixDeterminant(view), view);
	XMMATRIX invProj = XMMatrixInverse(&XMMatrixDeterminant(proj), proj);
	XMMATRIX invViewProj = XMMatrixInverse(&XMMatrixDeterminant(viewProj), viewProj);

	XMStoreFloat4x4(&mMainPassCB.View, XMMatrixTranspose(view));
	XMStoreFloat4x4(&mMainPassCB.InvView, XMMatrixTranspose(invView));
	XMStoreFloat4x4(&mMainPassCB.Proj, XMMatrixTranspose(proj));
	XMStoreFloat4x4(&mMainPassCB.InvProj, XMMatrixTranspose(invProj));
	XMStoreFloat4x4(&mMainPassCB.ViewProj, XMMatrixTranspose(viewProj));
	XMStoreFloat4x4(&mMainPassCB.InvViewProj, XMMatrixTranspose(invViewProj));
	mMainPassCB.EyePosW = mEyePos;
	mMainPassCB.RenderTargetSize = XMFLOAT2((float)mClientWidth, (float)mClientHeight);
	mMainPassCB.InvRenderTargetSize = XMFLOAT2(1.0f / mClientWidth, 1.0f / mClientHeight);
	mMainPassCB.NearZ = 1.0f;
	mMainPassCB.FarZ = 1000.0f;
	mMainPassCB.TotalTime = gt.TotalTime();
	mMainPassCB.DeltaTime = gt.DeltaTime();
	mMainPassCB.AmbientLight = { 0.25f, 0.25f, 0.35f, 1.0f };
	mMainPassCB.Lights[0].Direction = { 0.57735f, -0.57735f, 0.57735f };
	mMainPassCB.Lights[0].Strength = { 0.6f, 0.6f, 0.6f };
	mMainPassCB.Lights[1].Direction = { -0.57735f, -0.57735f, 0.57735f };
	mMainPassCB.Lights[1].Strength = { 0.3f, 0.3f, 0.3f };
	mMainPassCB.Lights[2].Direction = { 0.0f, -0.707f, -0.707f };
	mMainPassCB.Lights[2].Strength = { 0.15f, 0.15f, 0.15f };

	auto currPassCB = mCurrFrameResource
		->GetResource<Ubpa::DX12::ArrayUploadBuffer<PassConstants>>("gbPass constants");
	currPassCB->Set(0, mMainPassCB);
}

void HW08App::LoadTextures()
{
	std::array<std::wstring_view, 3> ironTextures{
		L"../data/textures/iron/albedo.dds",
		L"../data/textures/iron/roughness.dds",
		L"../data/textures/iron/metalness.dds"
	};

	Ubpa::DXRenderer::Instance().RegisterDDSTextureArrayFromFile(
		Ubpa::DXRenderer::Instance().GetUpload(),
		"iron",
		ironTextures.data(), ironTextures.size());
}

void HW08App::BuildRootSignature()
{
	{ // geometry
		CD3DX12_DESCRIPTOR_RANGE texTable;
		texTable.Init(D3D12_DESCRIPTOR_RANGE_TYPE_SRV, 3, 0);

		// Root parameter can be a table, root descriptor or root constants.
		CD3DX12_ROOT_PARAMETER slotRootParameter[4];

		// Perfomance TIP: Order from most frequent to least frequent.
		slotRootParameter[0].InitAsDescriptorTable(1, &texTable, D3D12_SHADER_VISIBILITY_PIXEL);
		slotRootParameter[1].InitAsConstantBufferView(0);
		slotRootParameter[2].InitAsConstantBufferView(1);
		slotRootParameter[3].InitAsConstantBufferView(2);

		auto staticSamplers = GetStaticSamplers();

		// A root signature is an array of root parameters.
		CD3DX12_ROOT_SIGNATURE_DESC rootSigDesc(4, slotRootParameter,
			(UINT)staticSamplers.size(), staticSamplers.data(),
			D3D12_ROOT_SIGNATURE_FLAG_ALLOW_INPUT_ASSEMBLER_INPUT_LAYOUT);

		Ubpa::DXRenderer::Instance().RegisterRootSignature("geometry", &rootSigDesc);
	}

	{ // screen
		CD3DX12_DESCRIPTOR_RANGE texTable;
		texTable.Init(D3D12_DESCRIPTOR_RANGE_TYPE_SRV, 1, 0);

		// Root parameter can be a table, root descriptor or root constants.
		CD3DX12_ROOT_PARAMETER slotRootParameter[1];

		// Perfomance TIP: Order from most frequent to least frequent.
		slotRootParameter[0].InitAsDescriptorTable(1, &texTable, D3D12_SHADER_VISIBILITY_PIXEL);

		auto staticSamplers = GetStaticSamplers();

		// A root signature is an array of root parameters.
		CD3DX12_ROOT_SIGNATURE_DESC rootSigDesc(1, slotRootParameter,
			(UINT)staticSamplers.size(), staticSamplers.data(),
			D3D12_ROOT_SIGNATURE_FLAG_ALLOW_INPUT_ASSEMBLER_INPUT_LAYOUT);

		Ubpa::DXRenderer::Instance().RegisterRootSignature("screen", &rootSigDesc);
	}
	{ // defer lighting
		CD3DX12_DESCRIPTOR_RANGE texTable;
		texTable.Init(D3D12_DESCRIPTOR_RANGE_TYPE_SRV, 3, 0);

		// Root parameter can be a table, root descriptor or root constants.
		CD3DX12_ROOT_PARAMETER slotRootParameter[4];

		// Perfomance TIP: Order from most frequent to least frequent.
		slotRootParameter[0].InitAsDescriptorTable(1, &texTable, D3D12_SHADER_VISIBILITY_PIXEL);
		slotRootParameter[1].InitAsConstantBufferView(0);
		slotRootParameter[2].InitAsConstantBufferView(1);
		slotRootParameter[3].InitAsConstantBufferView(2);

		auto staticSamplers = GetStaticSamplers();

		// A root signature is an array of root parameters.
		CD3DX12_ROOT_SIGNATURE_DESC rootSigDesc(4, slotRootParameter,
			(UINT)staticSamplers.size(), staticSamplers.data(),
			D3D12_ROOT_SIGNATURE_FLAG_ALLOW_INPUT_ASSEMBLER_INPUT_LAYOUT);

		Ubpa::DXRenderer::Instance().RegisterRootSignature("defer lighting", &rootSigDesc);
	}
}

void HW08App::BuildDescriptorHeaps()
{
}

void HW08App::BuildShadersAndInputLayout()
{
	Ubpa::DXRenderer::Instance().RegisterShaderByteCode("standardVS",
		L"..\\data\\shaders\\Default.hlsl", nullptr, "VS", "vs_5_0");
	Ubpa::DXRenderer::Instance().RegisterShaderByteCode("opaquePS",
		L"..\\data\\shaders\\Default.hlsl", nullptr, "PS", "ps_5_0");
	Ubpa::DXRenderer::Instance().RegisterShaderByteCode("screenVS",
		L"..\\data\\shaders\\Screen.hlsl", nullptr, "VS", "vs_5_0");
	Ubpa::DXRenderer::Instance().RegisterShaderByteCode("screenPS",
		L"..\\data\\shaders\\Screen.hlsl", nullptr, "PS", "ps_5_0");
	Ubpa::DXRenderer::Instance().RegisterShaderByteCode("geometryVS",
		L"..\\data\\shaders\\Geometry.hlsl", nullptr, "VS", "vs_5_0");
	Ubpa::DXRenderer::Instance().RegisterShaderByteCode("geometryPS",
		L"..\\data\\shaders\\Geometry.hlsl", nullptr, "PS", "ps_5_0");
	Ubpa::DXRenderer::Instance().RegisterShaderByteCode("deferLightingVS",
		L"..\\data\\shaders\\deferLighting.hlsl", nullptr, "VS", "vs_5_0");
	Ubpa::DXRenderer::Instance().RegisterShaderByteCode("deferLightingPS",
		L"..\\data\\shaders\\deferLighting.hlsl", nullptr, "PS", "ps_5_0");

    mInputLayout =
    {
        { "POSITION", 0, DXGI_FORMAT_R32G32B32_FLOAT, 0, 0, D3D12_INPUT_CLASSIFICATION_PER_VERTEX_DATA, 0 },
        { "NORMAL", 0, DXGI_FORMAT_R32G32B32_FLOAT, 0, 12, D3D12_INPUT_CLASSIFICATION_PER_VERTEX_DATA, 0 },
		{ "TEXCOORD", 0, DXGI_FORMAT_R32G32_FLOAT, 0, 24, D3D12_INPUT_CLASSIFICATION_PER_VERTEX_DATA, 0 }
    };
}

void HW08App::BuildShapeGeometry()
{
	std::vector<Vertex> start_vertices;
	std::vector<Vertex> end_vertices;
	std::vector<std::vector<Vertex>> interp_vertices_arr(InterpNum);
	std::vector<std::uint32_t> indices;

	auto obj_source = std::make_unique<Ubpa::TriMesh>("../data/meshes/horse_source.obj");
	auto obj_target = std::make_unique<Ubpa::TriMesh>("../data/meshes/horse_target.obj");
	/*auto obj_source = std::make_unique<Ubpa::TriMesh>("../data/meshes/leaf_source.obj");
	auto obj_target = std::make_unique<Ubpa::TriMesh>("../data/meshes/leaf_target.obj");*/
	/*auto obj_source = std::make_unique<Ubpa::TriMesh>("../data/meshes/rabbit_source.obj");
	auto obj_target = std::make_unique<Ubpa::TriMesh>("../data/meshes/rabbit_target.obj");*/

	/*obj_source->CombineSamePositionVertex();
	obj_source->ScaleToUnit();
	obj_target->CombineSamePositionVertex();
	obj_target->ScaleToUnit();*/

	start_vertices.resize(obj_source->VertexNumber());
	end_vertices.resize(obj_source->VertexNumber());
	for (auto& vertices : interp_vertices_arr)
		vertices.resize(obj_source->VertexNumber());
	indices.resize(3 * obj_source->TriangleNumber());
	for (size_t i = 0; i < obj_source->VertexNumber(); i++) {
		start_vertices[i].Pos = obj_source->positions[i].as<XMFLOAT3>();
		end_vertices[i].Pos   = obj_target->positions[i].as<XMFLOAT3>();

		start_vertices[i].Normal = obj_source->normals[i].as<XMFLOAT3>();
		for (auto& vertices : interp_vertices_arr)
			vertices[i].Normal = obj_target->normals[i].as<XMFLOAT3>();
		end_vertices[i].Normal = obj_target->normals[i].as<XMFLOAT3>();

		start_vertices[i].TexC = obj_source->texcoords[i].as<XMFLOAT2>();
		for (auto& vertices : interp_vertices_arr)
			vertices[i].TexC = obj_target->texcoords[i].as<XMFLOAT2>();
		end_vertices[i].TexC = obj_target->texcoords[i].as<XMFLOAT2>();
	}
	for (size_t i = 0; i < obj_source->TriangleNumber(); i++) {
		indices[3 * i + 0] = obj_source->indices[i][0];
		indices[3 * i + 1] = obj_source->indices[i][1];
		indices[3 * i + 2] = obj_source->indices[i][2];
	}

	Ubpa::HEMesh<Ubpa::HEMeshTriats_EmptyE<V, F, H>> hemesh(std::vector<size_t>{indices.begin(), indices.end()}, 3);
	assert(hemesh.IsValid() && hemesh.IsTriMesh() && hemesh.NumBoundaries() == 1);

	for (size_t i = 0; i < start_vertices.size(); i++) {
		hemesh.Vertices().at(i)->p = obj_source->positions[i].cast_to<Ubpa::pointf2>();
		hemesh.Vertices().at(i)->q = obj_target->positions[i].cast_to<Ubpa::pointf2>();
	}

	// compute area, cot_theta
	for (auto tri : hemesh.Polygons()) {
		auto he01 = tri->HalfEdge();
		auto he12 = he01->Next();
		auto he20 = he12->Next();

		auto v0 = he01->Origin();
		auto v1 = he12->Origin();
		auto v2 = he20->Origin();

		auto p0_p1 = v1->p - v0->p;
		auto p0_p2 = v2->p - v0->p;
		auto p1_p2 = v2->p - v1->p;
		auto p1_p0 = -p0_p1;
		auto p2_p0 = -p0_p2;
		auto p2_p1 = -p1_p2;

		tri->area = 0.5f * std::sqrt(std::max(0.f,1 - Ubpa::pow2(p0_p1.cos_theta(p0_p2)))) * p0_p1.norm() * p0_p2.norm();

		he01->cot_theta = p2_p0.cot_theta(p2_p1);
		he12->cot_theta = p0_p1.cot_theta(p0_p2);
		he20->cot_theta = p1_p2.cot_theta(p1_p0);
	}

	for (auto tri : hemesh.Polygons())
		tri->Init();

	// fix 1 vertex
	auto fix_v0 = hemesh.Vertices().front();
	auto fix_idx0 = static_cast<int>(hemesh.Index(fix_v0));

	// pre-compute A
	size_t N = hemesh.Vertices().size();
	Eigen::SparseMatrix<float> global_A(N, N);
	std::vector<Eigen::Triplet<float>> global_triplets;
	global_triplets.emplace_back(fix_idx0, fix_idx0, 1.f);
	for (auto vi : hemesh.Vertices()) {
		auto i = static_cast<int>(hemesh.Index(vi));
		if (vi == fix_v0)
			continue;
		float diag = 0.f;
		for (auto he_ij : vi->OutHalfEdges()) {
			auto he_ji = he_ij->Pair();
			auto vj = he_ji->Origin();
			auto j = static_cast<int>(hemesh.Index(vj));
			float cot_theta_ij = he_ij->cot_theta; // if he_ij is boundary, cot_theta_ij == 0
			float cot_theta_ji = he_ji->cot_theta; // if he_ji is boundary, cot_theta_ji == 0
			global_triplets.emplace_back(i, j, -(cot_theta_ij + cot_theta_ji));

			diag += cot_theta_ij + cot_theta_ji;
		}
		global_triplets.emplace_back(i, i, diag);
	}
	global_A.setFromTriplets(global_triplets.begin(), global_triplets.end());
	Eigen::SparseLU<Eigen::SparseMatrix<float>, Eigen::COLAMDOrdering<int>> global_solver;
	global_solver.analyzePattern(global_A);
	global_solver.factorize(global_A);

	for (size_t curInterp = 0; curInterp < InterpNum; curInterp++) {
		float t = static_cast<float>(curInterp) / static_cast<float>(InterpNum);

		// local
		for (auto tri : hemesh.Polygons())
			tri->UpdateLt(t);

		// global
		Eigen::MatrixXf global_B(N, 2);
		global_B.setZero();
		auto fix_v0_t = fix_v0->p.lerp(fix_v0->q, t);
		global_B(fix_idx0, 0) = fix_v0_t[0];
		global_B(fix_idx0, 1) = fix_v0_t[1];
		for (auto vi : hemesh.Vertices()) {
			if (vi == fix_v0)
				continue;

			auto i = static_cast<int>(hemesh.Index(vi));
			for (auto he_ij : vi->OutHalfEdges()) {
				auto he_ji = he_ij->Pair();
				auto vj = he_ji->Origin();

				if (he_ij->Polygon() != nullptr) {
					auto w_dp_ij = he_ij->cot_theta * he_ij->Polygon()->Lt * (vi->p - vj->p);
					global_B(i, 0) += w_dp_ij[0];
					global_B(i, 1) += w_dp_ij[1];
				}

				if (he_ji->Polygon() != nullptr) {
					auto w_dp_ji = he_ji->cot_theta * he_ji->Polygon()->Lt * (vj->p - vi->p);
					global_B(i, 0) -= w_dp_ji[0];
					global_B(i, 1) -= w_dp_ji[1];
				}
			}
		}
		Eigen::MatrixXf X = global_solver.solve(global_B);
		for (auto v : hemesh.Vertices()) {
			auto idx = hemesh.Index(v);
			interp_vertices_arr[curInterp][idx].Pos.x = X(idx, 0);
			interp_vertices_arr[curInterp][idx].Pos.y = X(idx, 1);
			interp_vertices_arr[curInterp][idx].Pos.z = 0.f;
		}
	}

	Ubpa::DX12::SubmeshGeometry objSubmesh;
	objSubmesh.IndexCount = obj_source->indices.size() * 3;
	objSubmesh.StartIndexLocation = 0;
	objSubmesh.BaseVertexLocation = 0;
	Ubpa::DXRenderer::Instance()
		.RegisterStaticMeshGeometry(
			Ubpa::DXRenderer::Instance().GetUpload(), "start_objGeo",
			start_vertices.data(), (UINT)start_vertices.size(), sizeof(Vertex),
			indices.data(), (UINT)indices.size(), DXGI_FORMAT_R32_UINT)
		.submeshGeometries["obj"] = objSubmesh;
	Ubpa::DXRenderer::Instance()
		.RegisterStaticMeshGeometry(
			Ubpa::DXRenderer::Instance().GetUpload(), "end_objGeo",
			end_vertices.data(), (UINT)end_vertices.size(), sizeof(Vertex),
			indices.data(), (UINT)indices.size(), DXGI_FORMAT_R32_UINT)
		.submeshGeometries["obj"] = objSubmesh;
	for (size_t i = 0; i < InterpNum; i++) {
		Ubpa::DXRenderer::Instance()
			.RegisterStaticMeshGeometry(
				Ubpa::DXRenderer::Instance().GetUpload(), "interp_" + std::to_string(i) + "_objGeo",
				interp_vertices_arr[i].data(), (UINT)interp_vertices_arr[i].size(), sizeof(Vertex),
				indices.data(), (UINT)indices.size(), DXGI_FORMAT_R32_UINT)
			.submeshGeometries["obj"] = objSubmesh;
	}

	trimeshes.emplace("obj_source", std::move(obj_source));
	trimeshes.emplace("obj_target", std::move(obj_target));
}

void HW08App::BuildPSOs()
{
	auto screenPsoDesc = Ubpa::DX12::Desc::PSO::Basic(
		Ubpa::DXRenderer::Instance().GetRootSignature("screen"),
		nullptr, 0,
		Ubpa::DXRenderer::Instance().GetShaderByteCode("screenVS"),
		Ubpa::DXRenderer::Instance().GetShaderByteCode("screenPS"),
		mBackBufferFormat,
		DXGI_FORMAT_UNKNOWN
	);
	Ubpa::DXRenderer::Instance().RegisterPSO("screen", &screenPsoDesc);

	auto geometryPsoDesc = Ubpa::DX12::Desc::PSO::MRT(
		Ubpa::DXRenderer::Instance().GetRootSignature("geometry"),
		mInputLayout.data(), (UINT)mInputLayout.size(),
		Ubpa::DXRenderer::Instance().GetShaderByteCode("geometryVS"),
		Ubpa::DXRenderer::Instance().GetShaderByteCode("geometryPS"),
		3,
		DXGI_FORMAT_R32G32B32A32_FLOAT,
		mDepthStencilFormat
	);
	geometryPsoDesc.RasterizerState.FillMode = D3D12_FILL_MODE_WIREFRAME;
	Ubpa::DXRenderer::Instance().RegisterPSO("geometry", &geometryPsoDesc);

	auto deferLightingPsoDesc = Ubpa::DX12::Desc::PSO::Basic(
		Ubpa::DXRenderer::Instance().GetRootSignature("defer lighting"),
		nullptr, 0,
		Ubpa::DXRenderer::Instance().GetShaderByteCode("deferLightingVS"),
		Ubpa::DXRenderer::Instance().GetShaderByteCode("deferLightingPS"),
		mBackBufferFormat,
		DXGI_FORMAT_UNKNOWN
	);
	Ubpa::DXRenderer::Instance().RegisterPSO("defer lighting", &deferLightingPsoDesc);
}

void HW08App::BuildFrameResources()
{
    for(int i = 0; i < gNumFrameResources; ++i)
    {
		auto fr = std::make_unique<Ubpa::DX12::FrameResource>(mFence.Get());

		ID3D12CommandAllocator* allocator;
		ThrowIfFailed(uDevice->CreateCommandAllocator(
			D3D12_COMMAND_LIST_TYPE_DIRECT,
			IID_PPV_ARGS(&allocator)));

		fr->RegisterResource("CommandAllocator", allocator, [](void* allocator) {
			reinterpret_cast<ID3D12CommandAllocator*>(allocator)->Release();
		});

		fr->RegisterResource("gbPass constants",
			new Ubpa::DX12::ArrayUploadBuffer<PassConstants>{ uDevice.raw.Get(), 1, true });

		fr->RegisterResource("ArrayUploadBuffer<MaterialConstants>",
			new Ubpa::DX12::ArrayUploadBuffer<MaterialConstants>{ uDevice.raw.Get(), mMaterials.size(), true });

		fr->RegisterResource("ArrayUploadBuffer<ObjectConstants>",
			new Ubpa::DX12::ArrayUploadBuffer<ObjectConstants>{ uDevice.raw.Get(), mAllRitems.size(), true });

		auto fgRsrcMngr = new Ubpa::DX12::FG::RsrcMngr;
		fgRsrcMngr->Init(uGCmdList, uDevice);
		fr->RegisterResource("FrameGraphRsrcMngr", fgRsrcMngr);

		mFrameResources.emplace_back(std::move(fr));
    }
}

void HW08App::BuildMaterials()
{
	auto iron = std::make_unique<Material>();
	iron->Name = "iron";
	iron->MatCBIndex = 0;
	iron->DiffuseSrvGpuHandle = Ubpa::DXRenderer::Instance().GetTextureSrvGpuHandle("iron");
	iron->DiffuseAlbedo = XMFLOAT4(1.0f, 1.0f, 1.0f, 1.0f);
	iron->FresnelR0 = XMFLOAT3(0.05f, 0.05f, 0.05f);
	iron->Roughness = 0.2f;

	mMaterials["iron"] = std::move(iron);
}

void HW08App::BuildRenderItems()
{
	auto start_objRitem = std::make_unique<RenderItem>();
	start_objRitem->World = Ubpa::transformf(Ubpa::pointf3{ 1,1,0 }, Ubpa::quatf{ {0,1,0}, Ubpa::to_radian(180.f) }).as<XMFLOAT4X4>();
	start_objRitem->ObjCBIndex = 0;
	start_objRitem->Mat = mMaterials["iron"].get();
	start_objRitem->Geo = &Ubpa::DXRenderer::Instance().GetMeshGeometry("start_objGeo");
	start_objRitem->PrimitiveType = D3D11_PRIMITIVE_TOPOLOGY_TRIANGLELIST;
	start_objRitem->IndexCount = start_objRitem->Geo->submeshGeometries["obj"].IndexCount;
	start_objRitem->StartIndexLocation = start_objRitem->Geo->submeshGeometries["obj"].StartIndexLocation;
	start_objRitem->BaseVertexLocation = start_objRitem->Geo->submeshGeometries["obj"].BaseVertexLocation;
	mAllRitems.push_back(std::move(start_objRitem));

	auto end_objRitem = std::make_unique<RenderItem>();
	end_objRitem->World = Ubpa::transformf(Ubpa::pointf3{ 1,-1,0 }, Ubpa::quatf{ {0,1,0}, Ubpa::to_radian(180.f) }).as<XMFLOAT4X4>();
	end_objRitem->ObjCBIndex = 1;
	end_objRitem->Mat = mMaterials["iron"].get();
	end_objRitem->Geo = &Ubpa::DXRenderer::Instance().GetMeshGeometry("end_objGeo");
	end_objRitem->PrimitiveType = D3D11_PRIMITIVE_TOPOLOGY_TRIANGLELIST;
	end_objRitem->IndexCount = end_objRitem->Geo->submeshGeometries["obj"].IndexCount;
	end_objRitem->StartIndexLocation = end_objRitem->Geo->submeshGeometries["obj"].StartIndexLocation;
	end_objRitem->BaseVertexLocation = end_objRitem->Geo->submeshGeometries["obj"].BaseVertexLocation;
	mAllRitems.push_back(std::move(end_objRitem));

	for (size_t i = 0; i < InterpNum; i++) {
		auto interp_objRitem = std::make_unique<RenderItem>();
		interp_objRitem->World = Ubpa::transformf(Ubpa::pointf3{ -1,0,0 }, Ubpa::quatf{ {0,1,0}, Ubpa::to_radian(180.f) })
			.as<XMFLOAT4X4>();
		interp_objRitem->ObjCBIndex = 2 + i;
		interp_objRitem->Mat = mMaterials["iron"].get();
		interp_objRitem->Geo = &Ubpa::DXRenderer::Instance().GetMeshGeometry("interp_" + std::to_string(i) + "_objGeo");
		interp_objRitem->PrimitiveType = D3D11_PRIMITIVE_TOPOLOGY_TRIANGLELIST;
		interp_objRitem->IndexCount = interp_objRitem->Geo->submeshGeometries["obj"].IndexCount;
		interp_objRitem->StartIndexLocation = interp_objRitem->Geo->submeshGeometries["obj"].StartIndexLocation;
		interp_objRitem->BaseVertexLocation = interp_objRitem->Geo->submeshGeometries["obj"].BaseVertexLocation;
		mAllRitems.push_back(std::move(interp_objRitem));
	}

	// All the render items are opaque.
	for(auto& e : mAllRitems)
		mOpaqueRitems.push_back(e.get());
}

void HW08App::DrawRenderItems(ID3D12GraphicsCommandList* cmdList, const std::vector<RenderItem*>& ritems)
{
    UINT objCBByteSize = Ubpa::DX12::Util::CalcConstantBufferByteSize(sizeof(ObjectConstants));
    UINT matCBByteSize = Ubpa::DX12::Util::CalcConstantBufferByteSize(sizeof(MaterialConstants));

	auto objectCB = mCurrFrameResource
		->GetResource<Ubpa::DX12::ArrayUploadBuffer<ObjectConstants>>("ArrayUploadBuffer<ObjectConstants>")
		->GetResource();
	auto matCB = mCurrFrameResource
		->GetResource<Ubpa::DX12::ArrayUploadBuffer<MaterialConstants>>("ArrayUploadBuffer<MaterialConstants>")
		->GetResource();

    // For each render item...
    for(size_t i = 0; i < ritems.size(); ++i)
    {
        auto ri = ritems[i];

        cmdList->IASetVertexBuffers(0, 1, &ri->Geo->VertexBufferView());
        cmdList->IASetIndexBuffer(&ri->Geo->IndexBufferView());
        cmdList->IASetPrimitiveTopology(ri->PrimitiveType);

        D3D12_GPU_VIRTUAL_ADDRESS objCBAddress = objectCB->GetGPUVirtualAddress() + ri->ObjCBIndex*objCBByteSize;
		D3D12_GPU_VIRTUAL_ADDRESS matCBAddress = matCB->GetGPUVirtualAddress() + ri->Mat->MatCBIndex*matCBByteSize;

		cmdList->SetGraphicsRootDescriptorTable(0, ri->Mat->DiffuseSrvGpuHandle);
        cmdList->SetGraphicsRootConstantBufferView(1, objCBAddress);
        cmdList->SetGraphicsRootConstantBufferView(3, matCBAddress);

        cmdList->DrawIndexedInstanced(ri->IndexCount, 1, ri->StartIndexLocation, ri->BaseVertexLocation, 0);
    }
}

std::array<const CD3DX12_STATIC_SAMPLER_DESC, 6> HW08App::GetStaticSamplers()
{
	// Applications usually only need a handful of samplers.  So just define them all up front
	// and keep them available as part of the root signature.

	const CD3DX12_STATIC_SAMPLER_DESC pointWrap(
		0, // shaderRegister
		D3D12_FILTER_MIN_MAG_MIP_POINT, // filter
		D3D12_TEXTURE_ADDRESS_MODE_WRAP,  // addressU
		D3D12_TEXTURE_ADDRESS_MODE_WRAP,  // addressV
		D3D12_TEXTURE_ADDRESS_MODE_WRAP); // addressW

	const CD3DX12_STATIC_SAMPLER_DESC pointClamp(
		1, // shaderRegister
		D3D12_FILTER_MIN_MAG_MIP_POINT, // filter
		D3D12_TEXTURE_ADDRESS_MODE_CLAMP,  // addressU
		D3D12_TEXTURE_ADDRESS_MODE_CLAMP,  // addressV
		D3D12_TEXTURE_ADDRESS_MODE_CLAMP); // addressW

	const CD3DX12_STATIC_SAMPLER_DESC linearWrap(
		2, // shaderRegister
		D3D12_FILTER_MIN_MAG_MIP_LINEAR, // filter
		D3D12_TEXTURE_ADDRESS_MODE_WRAP,  // addressU
		D3D12_TEXTURE_ADDRESS_MODE_WRAP,  // addressV
		D3D12_TEXTURE_ADDRESS_MODE_WRAP); // addressW

	const CD3DX12_STATIC_SAMPLER_DESC linearClamp(
		3, // shaderRegister
		D3D12_FILTER_MIN_MAG_MIP_LINEAR, // filter
		D3D12_TEXTURE_ADDRESS_MODE_CLAMP,  // addressU
		D3D12_TEXTURE_ADDRESS_MODE_CLAMP,  // addressV
		D3D12_TEXTURE_ADDRESS_MODE_CLAMP); // addressW

	const CD3DX12_STATIC_SAMPLER_DESC anisotropicWrap(
		4, // shaderRegister
		D3D12_FILTER_ANISOTROPIC, // filter
		D3D12_TEXTURE_ADDRESS_MODE_WRAP,  // addressU
		D3D12_TEXTURE_ADDRESS_MODE_WRAP,  // addressV
		D3D12_TEXTURE_ADDRESS_MODE_WRAP,  // addressW
		0.0f,                             // mipLODBias
		8);                               // maxAnisotropy

	const CD3DX12_STATIC_SAMPLER_DESC anisotropicClamp(
		5, // shaderRegister
		D3D12_FILTER_ANISOTROPIC, // filter
		D3D12_TEXTURE_ADDRESS_MODE_CLAMP,  // addressU
		D3D12_TEXTURE_ADDRESS_MODE_CLAMP,  // addressV
		D3D12_TEXTURE_ADDRESS_MODE_CLAMP,  // addressW
		0.0f,                              // mipLODBias
		8);                                // maxAnisotropy

	return {
		pointWrap, pointClamp,
		linearWrap, linearClamp,
		anisotropicWrap, anisotropicClamp };
}


#include"random.hpp"
#include"stdio.h"
#include"stdlib.h"
#include"useful/useful_plots.h"
#include"time.h"
#include<boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include<boost/random/linear_congruential.hpp>
#include"Random.h"
#include"Boost_Random.h"
#include"Standard_Random.h"
#include"State_of_Art_Random.h"

int main()
{
	int i;
	srand(time(NULL));

	double results[1000000];
/*
	for(i=0;i<1000000;++i)
	{
		//results[i]= rand()%101;
		results[i]= (rand()/(float)RAND_MAX); //(0,1)
	}

	//printf("%d\n",RAND_MAX);

	plotHistogram("rand_results",results,1000000);
*/

	/*
	randomG* random= new randomG();

	random->randomize(0.345356);

	for(i=0;i<1000000;++i)
	{
		//results[i]= (int)random->uniform(0,100);
		results[i]= random->uniform((double)0.00,(double)1.00);
	}

	plotHistogram("goldberg_random_results",results,1000000);
*/

	Random* r= new State_of_Art_Random(42u);
	//Random* r= new Boost_Random(42u);
	//Random* r= new Standard_Random(42u);

//	boost::minstd_rand generator(42u);
//	boost::uniform_real<>uni_dist(0,1);
//	boost::variate_generator< boost::minstd_rand&, boost::uniform_real<> > uni(generator, uni_dist);

	for(i=0;i<1000000;++i)
	{
//		results[i]= uni();
		//results[i]= r->uniform(1,100);
		//results[i]= r->gaussian(0.0,0.125);
		results[i]= r->gaussian(0.0,0.06);
	}

	plotHistogram("boost_random_results",results,1000000);

	return 0;
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "grad.h"
#include <Eigen/Geometry>
#include <vector>

#include "per_face_normals.h"
#include "volume.h"
#include "doublearea.h"

template <typename DerivedV, typename DerivedF>
IGL_INLINE void grad_tet(const Eigen::PlainObjectBase<DerivedV>&V,
                     const Eigen::PlainObjectBase<DerivedF>&T,
                            Eigen::SparseMatrix<typename DerivedV::Scalar> &G,
                            bool uniform) {
  using namespace Eigen;
  assert(T.cols() == 4);
  const int n = V.rows(); int m = T.rows();

  /*
      F = [ ...
      T(:,1) T(:,2) T(:,3); ...
      T(:,1) T(:,3) T(:,4); ...
      T(:,1) T(:,4) T(:,2); ...
      T(:,2) T(:,4) T(:,3)]; */
  MatrixXi F(4*m,3);
  for (int i = 0; i < m; i++) {
    F.row(0*m + i) << T(i,0), T(i,1), T(i,2);
    F.row(1*m + i) << T(i,0), T(i,2), T(i,3);
    F.row(2*m + i) << T(i,0), T(i,3), T(i,1);
    F.row(3*m + i) << T(i,1), T(i,3), T(i,2);
  }
  // compute volume of each tet
  VectorXd vol; igl::volume(V,T,vol);

  VectorXd A(F.rows());
  MatrixXd N(F.rows(),3);
  if (!uniform) {
    // compute tetrahedron face normals
    igl::per_face_normals(V,F,N); int norm_rows = N.rows();
    for (int i = 0; i < norm_rows; i++)
      N.row(i) /= N.row(i).norm();
    igl::doublearea(V,F,A); A/=2.;
  } else {
    // Use a uniform tetrahedra as a reference, with the same volume as the original one:
    //
    // Use normals of the uniform tet (V = h*[0,0,0;1,0,0;0.5,sqrt(3)/2.,0;0.5,sqrt(3)/6.,sqrt(2)/sqrt(3)])
    //         0         0    1.0000
    //         0.8165   -0.4714   -0.3333
    //         0          0.9428   -0.3333
    //         -0.8165   -0.4714   -0.3333
    for (int i = 0; i < m; i++) {
      N.row(0*m+i) << 0,0,1;
      double a = sqrt(2)*std::cbrt(3*vol(i)); // area of a face in a uniform tet with volume = vol(i)
      A(0*m+i) = (pow(a,2)*sqrt(3))/4.;
    }
    for (int i = 0; i < m; i++) {
      N.row(1*m+i) << 0.8165,-0.4714,-0.3333;
      double a = sqrt(2)*std::cbrt(3*vol(i));
      A(1*m+i) = (pow(a,2)*sqrt(3))/4.;
    }
    for (int i = 0; i < m; i++) {
      N.row(2*m+i) << 0,0.9428,-0.3333;
      double a = sqrt(2)*std::cbrt(3*vol(i));
      A(2*m+i) = (pow(a,2)*sqrt(3))/4.;
    }
    for (int i = 0; i < m; i++) {
      N.row(3*m+i) << -0.8165,-0.4714,-0.3333;
      double a = sqrt(2)*std::cbrt(3*vol(i));
      A(3*m+i) = (pow(a,2)*sqrt(3))/4.;
    }

  }

  /*  G = sparse( ...
      [0*m + repmat(1:m,1,4) ...
       1*m + repmat(1:m,1,4) ...
       2*m + repmat(1:m,1,4)], ...
      repmat([T(:,4);T(:,2);T(:,3);T(:,1)],3,1), ...
      repmat(A./(3*repmat(vol,4,1)),3,1).*N(:), ...
      3*m,n);*/
  std::vector<Triplet<double> > G_t;
  for (int i = 0; i < 4*m; i++) {
    int T_j; // j indexes : repmat([T(:,4);T(:,2);T(:,3);T(:,1)],3,1)
    switch (i/m) {
      case 0:
        T_j = 3;
        break;
      case 1:
        T_j = 1;
        break;
      case 2:
        T_j = 2;
        break;
      case 3:
        T_j = 0;
        break;
    }
    int i_idx = i%m;
    int j_idx = T(i_idx,T_j);

    double val_before_n = A(i)/(3*vol(i_idx));
    G_t.push_back(Triplet<double>(0*m+i_idx, j_idx, val_before_n * N(i,0)));
    G_t.push_back(Triplet<double>(1*m+i_idx, j_idx, val_before_n * N(i,1)));
    G_t.push_back(Triplet<double>(2*m+i_idx, j_idx, val_before_n * N(i,2)));
  }
  G.resize(3*m,n);
  G.setFromTriplets(G_t.begin(), G_t.end());
}

template <typename DerivedV, typename DerivedF>
IGL_INLINE void grad_tri(const Eigen::PlainObjectBase<DerivedV>&V,
                     const Eigen::PlainObjectBase<DerivedF>&F,
                    Eigen::SparseMatrix<typename DerivedV::Scalar> &G,
                    bool uniform)
{
  Eigen::Matrix<typename DerivedV::Scalar,Eigen::Dynamic,3>
    eperp21(F.rows(),3), eperp13(F.rows(),3);

  for (int i=0;i<F.rows();++i)
  {
    // renaming indices of vertices of triangles for convenience
    int i1 = F(i,0);
    int i2 = F(i,1);
    int i3 = F(i,2);

    // #F x 3 matrices of triangle edge vectors, named after opposite vertices
    Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v32 = V.row(i3) - V.row(i2);
    Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v13 = V.row(i1) - V.row(i3);
    Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v21 = V.row(i2) - V.row(i1);
    Eigen::Matrix<typename DerivedV::Scalar, 1, 3> n = v32.cross(v13);
    // area of parallelogram is twice area of triangle
    // area of parallelogram is || v1 x v2 ||
    // This does correct l2 norm of rows, so that it contains #F list of twice
    // triangle areas
    double dblA = std::sqrt(n.dot(n));
    Eigen::Matrix<typename DerivedV::Scalar, 1, 3> u;
    if (!uniform) {
      // now normalize normals to get unit normals
      u = n / dblA;
    } else {
      // Abstract equilateral triangle v1=(0,0), v2=(h,0), v3=(h/2, (sqrt(3)/2)*h)

      // get h (by the area of the triangle)
      double h = sqrt( (dblA)/sin(M_PI / 3.0)); // (h^2*sin(60))/2. = Area => h = sqrt(2*Area/sin_60)

      Eigen::VectorXd v1,v2,v3;
      v1 << 0,0,0;
      v2 << h,0,0;
      v3 << h/2.,(sqrt(3)/2.)*h,0;

      // now fix v32,v13,v21 and the normal
      v32 = v3-v2;
      v13 = v1-v3;
      v21 = v2-v1;
      n = v32.cross(v13);
    }

    // rotate each vector 90 degrees around normal
    double norm21 = std::sqrt(v21.dot(v21));
    double norm13 = std::sqrt(v13.dot(v13));
    eperp21.row(i) = u.cross(v21);
    eperp21.row(i) = eperp21.row(i) / std::sqrt(eperp21.row(i).dot(eperp21.row(i)));
    eperp21.row(i) *= norm21 / dblA;
    eperp13.row(i) = u.cross(v13);
    eperp13.row(i) = eperp13.row(i) / std::sqrt(eperp13.row(i).dot(eperp13.row(i)));
    eperp13.row(i) *= norm13 / dblA;
  }

  std::vector<int> rs;
  rs.reserve(F.rows()*4*3);
  std::vector<int> cs;
  cs.reserve(F.rows()*4*3);
  std::vector<double> vs;
  vs.reserve(F.rows()*4*3);

  // row indices
  for(int r=0;r<3;r++)
  {
    for(int j=0;j<4;j++)
    {
      for(int i=r*F.rows();i<(r+1)*F.rows();i++) rs.push_back(i);
    }
  }

  // column indices
  for(int r=0;r<3;r++)
  {
    for(int i=0;i<F.rows();i++) cs.push_back(F(i,1));
    for(int i=0;i<F.rows();i++) cs.push_back(F(i,0));
    for(int i=0;i<F.rows();i++) cs.push_back(F(i,2));
    for(int i=0;i<F.rows();i++) cs.push_back(F(i,0));
  }

  // values
  for(int i=0;i<F.rows();i++) vs.push_back(eperp13(i,0));
  for(int i=0;i<F.rows();i++) vs.push_back(-eperp13(i,0));
  for(int i=0;i<F.rows();i++) vs.push_back(eperp21(i,0));
  for(int i=0;i<F.rows();i++) vs.push_back(-eperp21(i,0));
  for(int i=0;i<F.rows();i++) vs.push_back(eperp13(i,1));
  for(int i=0;i<F.rows();i++) vs.push_back(-eperp13(i,1));
  for(int i=0;i<F.rows();i++) vs.push_back(eperp21(i,1));
  for(int i=0;i<F.rows();i++) vs.push_back(-eperp21(i,1));
  for(int i=0;i<F.rows();i++) vs.push_back(eperp13(i,2));
  for(int i=0;i<F.rows();i++) vs.push_back(-eperp13(i,2));
  for(int i=0;i<F.rows();i++) vs.push_back(eperp21(i,2));
  for(int i=0;i<F.rows();i++) vs.push_back(-eperp21(i,2));

  // create sparse gradient operator matrix
  G.resize(3*F.rows(),V.rows());
  std::vector<Eigen::Triplet<typename DerivedV::Scalar> > triplets;
  for (int i=0;i<(int)vs.size();++i)
  {
    triplets.push_back(Eigen::Triplet<typename DerivedV::Scalar>(rs[i],cs[i],vs[i]));
  }
  G.setFromTriplets(triplets.begin(), triplets.end());
}

template <typename DerivedV, typename DerivedF>
IGL_INLINE void igl::grad(const Eigen::PlainObjectBase<DerivedV>&V,
                     const Eigen::PlainObjectBase<DerivedF>&F,
                    Eigen::SparseMatrix<typename DerivedV::Scalar> &G,
                    bool uniform)
{
  assert(F.cols() == 3 || F.cols() == 4);
  if (F.cols() == 3)
    return grad_tri(V,F,G,uniform);
  if (F.cols() == 4)
    return grad_tet(V,F,G,uniform);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::grad<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::SparseMatrix<Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar, 0, int>&, bool);
template void igl::grad<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::SparseMatrix<Eigen::Matrix<double, -1, 3, 0, -1, 3>::Scalar, 0, int>&, bool);
#endif


#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <random>
#include <Eigen/Cholesky>
#include <fstream>
#include <iomanip>
#include <srrg_system_utils/parse_command_line.h>
#include <srrg_boss/deserializer.h>

#include "srrg_solver/solver_core/instances.h"
#include "srrg_solver/solver_core/factor_graph.h"
#include "srrg_solver/variables_and_factors/types_3d/variable_se3_ad.h"
#include "srrg_solver/variables_and_factors/types_3d/variable_point3_ad.h"
#include "srrg_solver/variables_and_factors/types_3d/se3_pose_pose_geodesic_error_factor.h"
#include "srrg_solver/variables_and_factors/types_3d/se3_pose_point_offset_error_factor.h"

#include "srrg_solver/variables_and_factors/types_2d/variable_se2_ad.h"
#include "srrg_solver/variables_and_factors/types_2d/variable_point2_ad.h"
#include "srrg_solver/variables_and_factors/types_2d/se2_pose_pose_geodesic_error_factor.h"
#include "srrg_solver/variables_and_factors/types_2d/se2_pose_point_error_factor.h"

#include "srrg_solver/utils/solver_evaluator.h"

using namespace srrg2_core;
using namespace srrg2_solver;
using namespace std;

extern char** environ;
const std::string exe_name = environ[0];
#define LOG std::cerr << exe_name << "|"

static const char* banner[] = {
  "evaluates a  factor graph,  by registering the poses in the input graph with the corresponding poses of the gt",
  0
};

// ia THE PROGRAM
int main(int argc, char** argv) {
  using namespace std;
  ParseCommandLine cmd_line(argv, banner);
  ArgumentString input_file          (&cmd_line, "i",    "input-file",             "file where to read the input ", "");
  ArgumentString gt_file          (&cmd_line, "gt",    "gt-file",           "file where to read the ground truth", "");
  ArgumentString output_file          (&cmd_line, "o",    "output-file",             "file where to write the output ", "");
  ArgumentString ev_mode          (&cmd_line, "m",    "mode",           "eval mode, [se3, se2, sim3]", "se3");
  cmd_line.parse();

  FactorGraphPtr graph, gt_graph;

  std::cerr << "loading file: [" << gt_file.value() << "]... ";
  gt_graph = FactorGraph::read(gt_file.value());
  std::cerr << "done, factors:" << gt_graph->factors().size() << " vars: " << gt_graph->variables().size() << std::endl;

  std::cerr << "loading file: [" << input_file.value() << "]... ";
  graph = FactorGraph::read(input_file.value());
  std::cerr << "done, factors:" << graph->factors().size() << " vars: " << graph->variables().size() << std::endl;

  std::cerr <<  "Setting evaluator ...";
  SolverEvaluator evaluator;
  evaluator.setGroundTruth(*gt_graph);
  cerr << "done" << endl;
  if (ev_mode.value()=="se2") {
    evaluator.alignSE2(*graph, true);
  } else if (ev_mode.value()=="se3") {
    evaluator.alignSE3(*graph, true);
  } else if (ev_mode.value()=="sim3") {
    evaluator.alignSim3(*graph, true);
  } else {
    cerr << "unknown ev mode [" << ev_mode.value() << "]" << endl;
    return -1;
  }

  if (output_file.isSet()) {
    cerr << "saving transformed graph [" <<output_file.value() << "]" <<  endl;
    graph->setSerializationLevel(-1);
    graph->write(output_file.value());
  }
  return 0;
}


#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/Pose2_Point2_Factor.h>
#include <vector>
#include <fstream>
#include <string>
#include <time.h>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>

using namespace std;
using namespace gtsam;
using namespace boost::algorithm;

using symbol_shorthand::X;
using symbol_shorthand::L;

// Testing params
const size_t max_odom_count = 3800; //3800

//const bool is_with_ambiguity = false; //run original iSAM2 without ambiguities
const bool is_with_ambiguity = true; //run original iSAM2 with ambiguities

noiseModel::Diagonal::shared_ptr prior_noise_model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.0001, 0.0001, 0.0001).finished());

noiseModel::Diagonal::shared_ptr pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(3) << 1.0/100.0, 1.0/500.0, 1.0/500.0).finished());

noiseModel::Diagonal::shared_ptr point_noise_model = noiseModel::Diagonal::Sigmas((Vector(2) << 1.0/1.581139, 1.0/1.581139).finished());
/* ************************************************************************* */

int main(int argc, char* argv[]) {

  ifstream in("../data/mh_T2_victoriaPark_01.txt"); //Type #2 only
  //ifstream in("../data/mh_T1_T2_victoriaPark_08.txt"); //Type #1 + Type #2

  size_t odom_count = 0;
  size_t landmark_count = 0;

  std::list<double> time_list;

  ISAM2Params parameters;

  //parameters.optimizationParams = gtsam::ISAM2DoglegParams(0.1);  //_initialDelta = 1.0 //Dogleg NOT implemented yet!!!!
  parameters.optimizationParams = gtsam::ISAM2GaussNewtonParams(0.0); //_wildfireThreshold = 0.001

  parameters.relinearizeThreshold = 0.01;

  parameters.relinearizeSkip = 1;
  ISAM2* isam2 = new ISAM2(parameters); //[2] MHISAM2

  NonlinearFactorGraph* graph = new NonlinearFactorGraph();

  Values init_values;
  Values results;

  double x = 0.0;
  double y = 0.0;
  double rad = 0.0;

  Pose2 prior_pose(x, y, rad);

  init_values.insert(X(0), prior_pose);

  graph->add(PriorFactor<Pose2>(X(0), prior_pose, prior_noise_model));

  isam2->update(*graph, init_values);
  graph->resize(0);
  init_values.clear();
  results = isam2->calculateBestEstimate();

  //*
  size_t key_s = 0;
  size_t key_t = 0;

  clock_t start_time = clock();
  string str;
  while (getline(in, str) && odom_count < max_odom_count) {

    //cout << str << endl;
    vector<string> parts;
    split(parts, str, is_any_of(" "));

    if (parts[0] == "ODOMETRY") {
      key_s = stoi(parts[1]);
      key_t = stoi(parts[3]);
      int m_num = stoi(parts[5]);
      vector<double> x_arr(m_num);
      vector<double> y_arr(m_num);
      vector<double> rad_arr(m_num);
      for (int i = 0; i < m_num; ++i) {
        x_arr[i] = stod(parts[6 + 3*i]);
        y_arr[i] = stod(parts[7 + 3*i]);
        rad_arr[i] = stod(parts[8 + 3*i]);
      }

      Pose2 odom_pose;
      if (is_with_ambiguity) {
        // Get wrong intentionally
        int id = odom_count%m_num;
        odom_pose = Pose2(x_arr[id], y_arr[id], rad_arr[id]);
      } else {
        odom_pose = Pose2(x_arr[0], y_arr[0], rad_arr[0]);
      }

      init_values.insert(X(key_t), results.at<Pose2>(X(key_s))*odom_pose);

      graph->add(BetweenFactor<Pose2>(X(key_s), X(key_t), odom_pose, pose_noise_model));

      odom_count++;

    } else { //LANDMARK
      //*
      key_s = stoi(parts[1]);
      int k_num = stoi(parts[2]);

      if (k_num == 1) {
        key_t = stoi(parts[3]);
        //int m_num = 1;
        double l_x = stod(parts[6]);
        double l_y = stod(parts[7]);

        //Pose2 odom_pose(x_arr[0], y_arr[0], rad_arr[0]*180.0/M_PI);
        Point2 measured_point(l_x, l_y);

        if (key_t >= landmark_count) {
          init_values.insert(L(key_t), results.at<Pose2>(X(key_s))*measured_point); //operator*
          landmark_count++;
        }

        graph->add(Pose2_Point2_Factor(X(key_s), L(key_t), measured_point, point_noise_model));

      } else {

        if (is_with_ambiguity) {
          // Get wrong intentionally
          if (odom_count%4 != 0) {
            key_t = stoi(parts[4]);
          } else {
            key_t = stoi(parts[3]);
          }

        } else {
          key_t = stoi(parts[3]);
        }

        double l_x = stod(parts[7]);
        double l_y = stod(parts[8]);

        Point2 measured_point(l_x, l_y);

        graph->add(Pose2_Point2_Factor(X(key_s), L(key_t), measured_point, point_noise_model));

      }
      // */
    }

    isam2->update(*graph, init_values);
    graph->resize(0);
    init_values.clear();
    results = isam2->calculateBestEstimate();

    if (parts[0] == "ODOMETRY") {
      clock_t cur_time = clock();
      time_list.push_back(cur_time - start_time);
    }

    if (time_list.size()%100 == 0 && parts[0] == "ODOMETRY") {
      string step_file_idx = std::to_string(100000 + time_list.size());

      ofstream step_outfile;
      string step_file_name = "T2_step_files/ISAM2_TEST_victoriaPark_S" + step_file_idx;
      step_outfile.open(step_file_name + ".txt");
      for (size_t i = 0; i < (key_t + 1); ++i) {
        Pose2 out_pose = results.at<Pose2>(X(i));
        step_outfile << out_pose.x() << " " << out_pose.y() << " " << out_pose.theta() << endl;
      }
      step_outfile.close();
    }

  }

  clock_t end_time = clock();
  clock_t total_time = end_time - start_time;
  cout << "total_time: " << total_time << endl;
  //*

  ofstream outfile;
  string file_name = "ISAM2_TEST_victoriaPark";
  outfile.open(file_name + ".txt");

  for (size_t i = 0; i < odom_count; ++i) {
    Pose2 out_pose = results.at<Pose2>(X(i));

    outfile << out_pose.x() << " " << out_pose.y() << " " << out_pose.theta() << endl;
  }
  outfile.close();
  cout << "output " << file_name << ".txt file." << endl;
  // */
  ofstream lm_outfile;
  string lm_file_name = "ISAM2_TEST_victoriaPark_lm";
  lm_outfile.open(lm_file_name + ".txt");

  for (size_t i = 0; i < landmark_count; ++i) {
    Point2 out_point = results.at<Point2>(L(i));

    lm_outfile << out_point.x() << " " << out_point.y() << endl;
  }
  lm_outfile.close();
  cout << "output " << lm_file_name << ".txt file." << endl;

  //*
  ofstream outfile_time;
  string time_file_name = "ISAM2_TEST_victoriaPark_time";
  outfile_time.open(time_file_name + ".txt");
  for (auto acc_time : time_list) {
    outfile_time << acc_time << endl; //WRONG FORMAT... JUST FOR SIMPLE SHOW
  }
  outfile_time.close();
  cout << "output " << time_file_name << ".txt file." << endl;
  // */
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// NeoHookeanEnergy.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  Volumetric and plane stress Neo-Hookean energy.
*///////////////////////////////////////////////////////////////////////////////
#ifndef NEOHOOKEANENERGY_HH
#define NEOHOOKEANENERGY_HH

#include <Eigen/Dense>
#include <cstdlib>

#include <MeshFEM/EnergyDensities/Tensor.hh>
#include <MeshFEM/EnergyDensities/EnergyTraits.hh>
#include <MeshFEM/EnergyDensities/EDensityAdaptors.hh>

/**
 *  Implements the Neo-Hookean Energy described in the Neo-Hookean Energy section of doc/doc.pdf
 */
template<typename _Real, size_t _Dim, template<typename, size_t> class _Derived_T>
struct NeoHookeanEnergyBase : public Concepts::NeoHookeanEnergy
{
    static constexpr size_t Dimension = _Dim;
    static constexpr size_t N         = _Dim;
    static constexpr EDensityType EDType = EDensityType::FBased;
    using Real = _Real;
    using Derived = _Derived_T<Real, N>;
    using Matrix = Eigen::Matrix<Real, N, N>;

    NeoHookeanEnergyBase(const NeoHookeanEnergyBase& other) = default;

    // Constructor copying material properties only, not the current deformation
    NeoHookeanEnergyBase(const NeoHookeanEnergyBase& other, UninitializedDeformationTag &&)
        : m_lambda(other.m_lambda), m_mu(other.m_mu), m_finite_continuation_start(other.m_finite_continuation_start)
    { }

    // Construct from Lame's first parameter (lambda) and shear modulus (mu).
    NeoHookeanEnergyBase(Real lambda, Real mu, Real finite_continuation_start = -1)
        : m_lambda(lambda), m_mu(mu), m_finite_continuation_start(finite_continuation_start)
    {
        setDeformationGradient(Matrix::Identity());
    }

    void setDeformationGradient(const Matrix& deformation_gradient, const EvalLevel /* elevel */ = EvalLevel::Full) {
        m_F = deformation_gradient;
        m_detF = deformation_gradient.determinant();
        m_Finv = m_F.inverse();
    }

    const Matrix &getDeformationGradient() const { return m_F; }

    Real energy() const {
        // Standard behavior: return inf for inverted elements
        if (m_finite_continuation_start <= 0 && m_detF < 0) {
            return std::numeric_limits<Real>::max();
        }

        const Real I3 = getI3();
        const Real I1 = getI1();

        // Modified behavior to support inverted elements:
        // if det F < eps, we replace the log(I3) term by a constant + exp(- (det (F) - eps) )
        // where the constant is chosen such that the energy remains continuous
        if (m_finite_continuation_start > 0 && m_detF < m_finite_continuation_start) {
            Derived tmp(m_lambda, m_mu, m_finite_continuation_start);
            Matrix tmp_F = Matrix::Identity();
            tmp_F(0, 0) = m_finite_continuation_start;
            tmp.setDeformationGradient(tmp_F);
            Real continuation_constant = - std::log(tmp.getI3()) * (m_lambda / 2 + m_mu) / 2;

            return m_lambda * (I3 - 1) / 4 + m_mu * (I1 - 3) / 2
                + continuation_constant + std::exp(-(m_detF - m_finite_continuation_start)) - 1;
        }

        return (m_mu / 2) * (I1 - 3) + (m_lambda / 4) * (I3 - 1) - std::log(I3) * (m_mu / 2 + m_lambda / 4);
    }

    Matrix denergy() const {
        if (m_finite_continuation_start > 0 && m_detF < m_finite_continuation_start) {
            Real dPsi3 = m_lambda / 4;
            return (-std::exp(-(m_detF - m_finite_continuation_start))) * m_detF * m_Finv.transpose()
                + dPsi3 * d_I3_d_F()
                + d_psi_d_I1() * d_I1_d_F();
        }

        return d_psi_d_I1() * d_I1_d_F() + d_psi_d_I3() * d_I3_d_F();
    }

    Real denergy(const Matrix& dF) const { return doubleContract(dF, denergy()); }

    Real d2energy(const Matrix& dF_a, const Matrix& dF_b) const {
        return doubleContract(dF_a, delta_denergy(dF_b));
    }

    // Directional derivative of "denergy" along dF:
    //      (d^2 psi / dF^2) : dF
    template<class Mat_>
    Matrix delta_denergy(const Mat_ &dF) const {
        if (m_finite_continuation_start > 0 && m_detF < m_finite_continuation_start) {
            // ln I3 term is constant, but exp(-(detF)) got added
            Real dPsi3 = m_lambda / 4;
            Real exp_term = -std::exp(-(m_detF - m_finite_continuation_start));

            Matrix d_det_dF = m_detF * m_Finv.transpose();
            Matrix delta_d_det_dF = doubleContract(d_det_dF, dF) * m_Finv.transpose()
                                  - m_detF * (m_Finv * dF * m_Finv).transpose();

            return exp_term * d_det_dF * d_det_dF
                + exp_term * delta_d_det_dF
                + dPsi3 * delta_d_I3_d_F(dF)
                + d_psi_d_I1() * delta_d_I1_d_F(dF);
        }

        Matrix dI3 = d_I3_d_F();
        Real delta_I3 = doubleContract(dI3, dF);
        return d_psi_d_I1() * delta_d_I1_d_F(dF) + (d2_psi_d2_I3() * delta_I3) * dI3 + d_psi_d_I3() * delta_d_I3_d_F(dF);
    }

    // (d^3 psi / dF^3) :: (dF_a \otimes dF_b)
    // Second variation of "denergy" along (dF_a, dF_b)
    template<class Mat_, class Mat2_>
    Matrix delta2_denergy(const Mat_ &dF_a, const Mat2_ &dF_b) const {
        if (m_finite_continuation_start > 0) throw std::runtime_error("Finite continuation energy variant is not supported");

        Matrix dI3 = d_I3_d_F();
        Real delta_I3_a = doubleContract(dI3, dF_a),
             delta_I3_b = doubleContract(dI3, dF_b);
        Matrix delta_dI3_a = delta_d_I3_d_F(dF_a),
               delta_dI3_b = delta_d_I3_d_F(dF_b);
        return // Derivative of (d_psi_d_I1() * delta_d_I1_d_F(dF):                      (Note d2_psi_d_I1 = 0)
               d_psi_d_I1() * delta2_d_I1_d_F(dF_a, dF_b)                                // Symmetric
               // Derivative of (d2_psi_d2_I3() * delta_I3) * dI3:
             + (d3_psi_d3_I3() * delta_I3_b * delta_I3_a) * dI3                          // Symmetric
             + (d2_psi_d2_I3() *              doubleContract(delta_dI3_b, dF_a)) * dI3   // Symmetric
             + (d2_psi_d2_I3() *              delta_I3_a) * delta_dI3_b                  // Symmetric pair (*)
               // Derivative of d_psi_d_I3() * delta_d_I3_d_F(dF):
             + (d2_psi_d2_I3() *              delta_I3_b) * delta_dI3_a                  // Symmetric pair (*)
             + (  d_psi_d_I3()                          ) * delta2_d_I3_d_F(dF_a, dF_b); // Symmetric
    }

    ////////////////////////////////////////////////////////////////////////////
    // Invariants of the Cauchy-Green deformation tensor and their derivatives
    // appearing in the energy density expressions.
    // These must be provided by the derived class since they differ for the
    // 2D plane stress and 3D volumetric cases.
    ////////////////////////////////////////////////////////////////////////////
    // Trace of Cauchy-Green deformation tensor.
    Real getI1() const { return derived().getI1(); }

    // Determinant of Cauchy-Green deformation tensor.
    Real getI3() const { return derived().getI3(); }

    // dI1/dF
    Matrix d_I1_d_F() const { return derived().d_I1_d_F(); }

    // dI3/dF
    Matrix d_I3_d_F() const { return derived().d_I3_d_F(); }

    // (d^2 I1 / dF^2) : dF

    template<class Mat_>
    Matrix delta_d_I1_d_F(const Mat_ &dF) const { return derived().delta_d_I1_d_F(dF); }

    // (d^2 I1 / dF^2) : dF
    template<class Mat_>
    Matrix delta_d_I3_d_F(const Mat_ &dF) const { return derived().delta_d_I3_d_F(dF); }

    // (d^3 I1 / dF^3) :: (dF_a \otimes dF_b)
    Matrix delta2_d_I1_d_F(const Matrix &dF_a, const Matrix &dF_b) const { return derived().delta2_d_I1_d_F(dF_a, dF_b); }

    // (d^3 I3 / dF^3) :: (dF_a \otimes dF_b)
    Matrix delta2_d_I3_d_F(const Matrix &dF_a, const Matrix &dF_b) const { return derived().delta2_d_I3_d_F(dF_a, dF_b); }

    Matrix PK2Stress() const { return m_Finv * denergy(); }

    const Derived &derived() const { return *static_cast<const Derived *>(this); }
protected:
    ////////////////////////////////////////////////////////////////////////////
    // Derivatives of the energy density with respect to the tensor invariants.
    ////////////////////////////////////////////////////////////////////////////
    // Derivative of the energy density with respect to I1
    Real d_psi_d_I1() const { return m_mu / 2; }

    // Derivative of the energy density with respect to I3
    Real d_psi_d_I3() const { return (m_lambda - (2 * m_mu + m_lambda) / getI3()) / 4; }

    // Second derivative of the energy density with respect to I3
    Real d2_psi_d2_I3() const {
        Real I3 = getI3();
        return (2 * m_mu + m_lambda) / (4 * I3 * I3);
    }

    // Third derivative of the energy density with respect to I3
    Real d3_psi_d3_I3() const {
        Real I3 = getI3();
        return - (m_mu + 0.5 * m_lambda) / (I3 * I3 * I3);
    }

    ////////////////////////////////////////////////////////////////////////////
    // Derivatives of the "unpadded" invariants
    // (i.e., the 2x2 invariants for 2D, not including the C33 component)
    ////////////////////////////////////////////////////////////////////////////
    Real           unpaddedI3()     const { return m_detF * m_detF; }
    Matrix       d_unpaddedI3_d_F() const { return (2 * unpaddedI3()) * m_Finv.transpose(); }
    template<typename Mat_>
    Real delta_unpaddedI3(const Mat_ &dF) const { return (2 * unpaddedI3()) * doubleContract(m_Finv.transpose(), dF); }
    template<typename Mat_>
    Matrix delta_d_unpaddedI3_d_F(const Mat_ &dF) const {
        return (2 * delta_unpaddedI3(dF)) *  m_Finv.transpose()
             - (2 *       unpaddedI3()  ) * (m_Finv * dF * m_Finv).transpose();
    }

    Matrix delta2_d_unpaddedI3_d_F(const Matrix &dF_a, const Matrix &dF_b) const {
        Real delta2_unpaddedI3_ab = doubleContract(delta_d_unpaddedI3_d_F(dF_a), dF_b);
        Matrix delta_Finv_a = -(m_Finv * dF_a * m_Finv),
               delta_Finv_b = -(m_Finv * dF_b * m_Finv);

        return (2 * delta2_unpaddedI3_ab)   *       m_Finv.transpose()
             + (2 * delta_unpaddedI3(dF_a)) * delta_Finv_b.transpose()
             + (2 * delta_unpaddedI3(dF_b)) * delta_Finv_a.transpose()
             - (2 *       unpaddedI3()  ) * (delta_Finv_b * dF_a * m_Finv).transpose()
             - (2 *       unpaddedI3()  ) * (m_Finv * dF_a * delta_Finv_b).transpose();
    }

    Real m_lambda = 0.0; // Lame's first parameter
    Real m_mu = 0.0;     // Shear modulus
    Real m_finite_continuation_start = -1;

    // Cached deformation quantities.
    Matrix m_F = Matrix::Identity(), m_Finv = Matrix::Identity();
    Real m_detF = 1.0;
};

template<typename _Real, size_t _Dim>
struct NeoHookeanEnergy;

template<typename _Real>
struct NeoHookeanEnergy<_Real, 2> : public NeoHookeanEnergyBase<_Real, 2, NeoHookeanEnergy>
{
    using Base = NeoHookeanEnergyBase<_Real, 2, ::NeoHookeanEnergy>;
    using Real = _Real;
    using Matrix = typename Base::Matrix;

    using Base::Base;

    NeoHookeanEnergy(const NeoHookeanEnergy &other)
        : Base(other), m_C33(other.m_C33) { }

    NeoHookeanEnergy &operator=(const NeoHookeanEnergy &other) = default; // Silence deprecation warning.

    void setDeformationGradient(const Matrix &F, const EvalLevel elevel = EvalLevel::Full) {
        Base::setDeformationGradient(F, elevel);
        m_C33 = (m_lambda + 2 * m_mu) / (m_lambda * unpaddedI3() + 2 * m_mu);
    }

    // Trace of full (padded) Cauchy-Green deformation tensor.
    Real getI1() const { return m_F.squaredNorm() + m_C33; }

    // Determinant of full (padded) Cauchy-Green deformation tensor.
    Real getI3() const { return unpaddedI3() * m_C33; }

    // dI1/dF
    Matrix d_I1_d_F() const { return 2 * m_F + d_C33_d_F(); }

    // dI3/dF
    Matrix d_I3_d_F() const { return d_unpaddedI3_d_F() * m_C33 + unpaddedI3() * d_C33_d_F(); }

    // (d^2 I1 / dF^2) : dF
    template<class Mat_>
    Matrix delta_d_I1_d_F(const Mat_ &dF) const { return 2 * dF.matrix() + delta_d_C33_d_F(dF); }

    // (d^2 I&1 / dF^2) : dF
    template<typename Mat_>
    Matrix delta_d_I3_d_F(const Mat_ &dF) const {
        Matrix dC33 = d_C33_d_F();
        Matrix d_unpaddedI3 = d_unpaddedI3_d_F();
        Real delta_unpaddedI3_val = doubleContract(d_unpaddedI3, dF);

        return delta_d_unpaddedI3_d_F(dF) * m_C33                    +
               d_unpaddedI3               * doubleContract(dC33, dF) +
               delta_unpaddedI3_val       * dC33                     +
               unpaddedI3()               * delta_d_C33_d_F(dF);
    }

    // (d^3 I1 / dF^3) :: (dF_a \otimes dF_b)
    Matrix delta2_d_I1_d_F(const Matrix &dF_a, const Matrix &dF_b) const { return delta2_d_C33_d_F(dF_a, dF_b); }

    // (d^3 I3 / dF^3) :: (dF_a \otimes dF_b)
    Matrix delta2_d_I3_d_F(const Matrix &dF_a, const Matrix &dF_b) const {
        Matrix dC33 = d_C33_d_F();
        Real delta_C33_a   = doubleContract(dC33, dF_a),
             delta_C33_b   = doubleContract(dC33, dF_b);
        Real delta2_C33_ab = doubleContract(delta_d_C33_d_F(dF_a), dF_b);
        Matrix delta_d_unpaddedI3_a  = delta_d_unpaddedI3_d_F(dF_a),
               delta_d_unpaddedI3_b  = delta_d_unpaddedI3_d_F(dF_b);
        Real    delta2_unpaddedI3_ab = doubleContract(delta_d_unpaddedI3_a, dF_b);

        Matrix d_unpaddedI3 = d_unpaddedI3_d_F();
        return // Derivative of delta_d_unpaddedI3_d_F(dF) * m_C33:
               delta2_d_unpaddedI3_d_F(dF_a, dF_b) * m_C33 + delta_d_unpaddedI3_a * delta_C33_b
               // Derivative of d_unpaddedI3 * doubleContract(dC33, dF):
             + delta_d_unpaddedI3_b * delta_C33_a + d_unpaddedI3 * delta2_C33_ab
               // Derivative of delta_unpaddedI3 * dC33:
             + delta2_unpaddedI3_ab * dC33 + doubleContract(d_unpaddedI3, dF_a) * delta_d_C33_d_F(dF_b)
               // Derivative of unpaddedI3() * delta_d_C33_d_F(dF):
             + doubleContract(d_unpaddedI3, dF_b) * delta_d_C33_d_F(dF_a) + unpaddedI3() * delta2_d_C33_d_F(dF_a, dF_b);
    }

protected:
    // Derivative of normal component C33 with respect to the 2D deformation gradient.
    Matrix d_C33_d_F() const { return d_C33_d_unpaddedI3() * d_unpaddedI3_d_F(); }

    template<class Mat_>
    Matrix delta_d_C33_d_F(const Mat_ &dF) const {
        return delta_d_C33_d_unpaddedI3(dF) * d_unpaddedI3_d_F() +
               d_C33_d_unpaddedI3()         * delta_d_unpaddedI3_d_F(dF);
    }

    Matrix delta2_d_C33_d_F(const Matrix &dF_a, const Matrix &dF_b) const {
        Matrix dC33 = d_C33_d_F();
        Real delta_C33_a   = doubleContract(dC33, dF_a),
             delta_C33_b   = doubleContract(dC33, dF_b);
        Real delta2_C33_ab = doubleContract(delta_d_C33_d_F(dF_a), dF_b);

        Real coeff = -2 * m_lambda / (m_lambda + 2 * m_mu);
        // Second variation of d_C33_d_unpaddedI3 along (dF_a, dF_b)
        Real delta2_d_C33_d_unpaddedI3_ab = coeff * (delta_C33_a * delta_C33_b + m_C33 * delta2_C33_ab);
        Real delta_d_C33_d_unpaddedI3_a   = coeff * m_C33 * delta_C33_a;
        Real delta_d_C33_d_unpaddedI3_b   = coeff * m_C33 * delta_C33_b;

        return delta2_d_C33_d_unpaddedI3_ab *        d_unpaddedI3_d_F()
             + delta_d_C33_d_unpaddedI3_a   *  delta_d_unpaddedI3_d_F(dF_b)
             + delta_d_C33_d_unpaddedI3_b   *  delta_d_unpaddedI3_d_F(dF_a)
             + d_C33_d_unpaddedI3()         * delta2_d_unpaddedI3_d_F(dF_a, dF_b);
    }

    // Derivative of normal component C33 with respect to the unpadded I3 invariant.
    Real d_C33_d_unpaddedI3() const {
        return -m_C33 * m_C33 * (m_lambda / (m_lambda + 2 * m_mu));
    }

    // Directional derivative of d_C33_d_unpaddedI3 along dF
    template<class Mat_>
    Real delta_d_C33_d_unpaddedI3(const Mat_ &dF) const {
        Real delta_C33 = doubleContract(d_C33_d_F(), dF);
        return -2 * m_C33 * delta_C33 * (m_lambda / (m_lambda + 2 * m_mu));
    }

private:
    using Base::m_F;
    using Base::m_detF;
    using Base::m_lambda;
    using Base::m_mu;
    using Base::unpaddedI3;
    using Base::d_unpaddedI3_d_F;
    using Base::delta_d_unpaddedI3_d_F;
    using Base::delta2_d_unpaddedI3_d_F;
    Real m_C33 = 1.0;
};

template<typename _Real>
struct NeoHookeanEnergy<_Real, 3> : public NeoHookeanEnergyBase<_Real, 3, NeoHookeanEnergy>
{
    using Base = NeoHookeanEnergyBase<_Real, 3, ::NeoHookeanEnergy>;
    using Real = _Real;
    using Matrix = typename Base::Matrix;
    using Base::Base;

    Real getI3() const { return m_detF * m_detF; }
    Real getI1() const { return m_F.squaredNorm(); }

    Matrix d_I1_d_F() const { return 2 * m_F; }
    Matrix d_I3_d_F() const { return this->d_unpaddedI3_d_F(); }

    template<typename Mat_> Matrix delta_d_I1_d_F(const Mat_ &dF) const { return 2 * dF.matrix(); }
    template<typename Mat_> Matrix delta_d_I3_d_F(const Mat_ &dF) const { return this->delta_d_unpaddedI3_d_F(dF); }

    Matrix delta2_d_I1_d_F(const Matrix &/* dF_a */, const Matrix /* &dF_b */) const { return Matrix::Zero(); }
    Matrix delta2_d_I3_d_F(const Matrix    &dF_a   , const Matrix    &dF_b   ) const { return this->delta2_d_unpaddedI3_d_F(dF_a, dF_b); }
private:
    using Base::m_F;
    using Base::m_detF;
    using Base::m_lambda;
    using Base::m_mu;
};

// Simulate a Neo-Hookean sheet made of a material with Poisson's ratio 0.5.
// This would cause the volumetric NeoHookeanEnergy above to blow up, but we
// can obtain a membrane energy by imposing incompressibility as a hard
// constraint.
template<typename _Real>
struct IncompressibleNeoHookeanEnergyCBased {
    static constexpr EDensityType EDType = EDensityType::CBased;
    static constexpr size_t Dimension = 2;
    static constexpr size_t N         = 2;
    using Real   = _Real;
    using Matrix = Eigen::Matrix<_Real, 2, 2>;
    using M2d    = Matrix;

    static constexpr const char *name() { return "IncompressibleNeoHookean"; }

    IncompressibleNeoHookeanEnergyCBased(const IncompressibleNeoHookeanEnergyCBased &other, UninitializedDeformationTag &&) {
        setYoungModulus(other.youngModulus());
    }

    IncompressibleNeoHookeanEnergyCBased(Real E = 6 /* corresponds to "stiffness"  of 1 */) {
        setYoungModulus(E);
    }

    void setC(const M2d &C) {
        Real a = C(0, 0),
             b = C(0, 1),
             c = C(1, 1);
        if (std::abs(b - C(1, 0)) > 1e-15) throw std::runtime_error("Asymmetric matrix");

        m_C = C;
        m_trace_C = C.trace();
        m_det_C = a * c - b * b;
        m_grad_det_C <<  c, -b,
                        -b,  a;
    }

    Real energy() const {
        return stiffness * (m_trace_C + 1.0 / m_det_C - 3.0);
    }

    // dpsi / dE = 2 * dpsi/dC
    M2d PK2Stress() const {
        return 2 * stiffness * (M2d::Identity() - (1.0 / (m_det_C * m_det_C)) * m_grad_det_C);
    }

    template<class Mat_>
    M2d delta_PK2Stress(const Mat_ &dC) const {
        M2d adj_dC;
        adj_dC << dC(1, 1), -dC(0, 1),
                 -dC(1, 0),  dC(0, 0);
        return 2 * (((2.0 * stiffness / (m_det_C * m_det_C * m_det_C)) * doubleContract(m_grad_det_C, dC)) * m_grad_det_C
                         - (stiffness / (m_det_C * m_det_C))                                               * adj_dC);
    }

    template<class Mat_, class Mat2_>
    Matrix delta2_PK2Stress(const Mat_ &/* dC_a */, const Mat2_ &/* dC_b */) const {
        throw std::runtime_error("Unimplemented");
    }

    // The stiffness parameter is mu / 2 = E / (4 * (1 + nu)) = E / 6
    void setYoungModulus(Real E) {
        stiffness = E / 6;
    }

    Real youngModulus() const {
        return stiffness * 6;
    }

    void copyMaterialProperties(const IncompressibleNeoHookeanEnergyCBased &other) { setYoungModulus(other.youngModulus()); }

    Real stiffness = 1.0; // mu / 2

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
    Real m_trace_C, m_det_C;
    M2d m_grad_det_C;
    M2d m_C;
};

template <typename _Real>
using IncompressibleNeoHookeanEnergy = EnergyDensityFBasedFromCBased<IncompressibleNeoHookeanEnergyCBased<_Real>>;

#endif


#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core>

#ifndef _MATRIX_FREE_OP_
#define _MATRIX_FREE_OP_

class MatrixFreeOperator;

namespace Eigen { namespace internal {
		template<>
		struct traits<MatrixFreeOperator> : public Eigen::internal::traits<Eigen::MatrixXd> {};
	}
}

class MatrixFreeOperator : public Eigen::EigenBase<Eigen::MatrixXd>
{
	public:

		typedef double Scalar;
		typedef double RealScalar;
		typedef int StorageIndex;

		enum {
			ColsAtCompileTime = Eigen::Dynamic,
			MaxColsAtCompileTime = Eigen::Dynamic,
			IsRowMajor = false
		};

		Index rows() const {return this-> _size;}
		Index cols() const {return this-> _size;}

		template<typename Vtype>
		Eigen::Product<MatrixFreeOperator,Vtype,Eigen::AliasFreeProduct> operator*(const Eigen::MatrixBase<Vtype>& x) const {
			return Eigen::Product<MatrixFreeOperator,Vtype,Eigen::AliasFreeProduct>(*this, x.derived());
		}

		// custom API
		MatrixFreeOperator();

		// convenience function
		Eigen::MatrixXd get_full_mat() const;
		Eigen::VectorXd diagonal() const;
		int get_size() const {return this->_size;}
		void set_size(int N) {this->_size = N;}

		// extract row/col of the operator
		virtual Eigen::VectorXd col(int index) const = 0;
		Eigen::VectorXd diag_el;

	protected:

		int _size;
};

namespace Eigen{

	namespace internal{

		// replacement of the mat*vect operation
		template<typename Vtype>
		struct generic_product_impl<MatrixFreeOperator, Vtype, DenseShape, DenseShape, GemvProduct>
		: generic_product_impl_base<MatrixFreeOperator,Vtype,generic_product_impl<MatrixFreeOperator,Vtype>>
		{

			typedef typename Product<MatrixFreeOperator,Vtype>::Scalar Scalar;

			template<typename Dest>
			static void scaleAndAddTo(Dest& dst, const MatrixFreeOperator& op, const Vtype &v, const Scalar& alpha)
			{
				//returns dst = alpha * op * v
				// alpha must be 1 here
				assert(alpha==Scalar(1) && "scaling is not implemented");
				EIGEN_ONLY_USED_FOR_DEBUG(alpha);

				// make the mat vect product
				for (int i=0; i<op.cols(); i++)
					dst += v(i) * op.col(i);
			}
		};

		// replacement of the operator*matrix operation
		template<typename Mtype>
		struct generic_product_impl<MatrixFreeOperator, Mtype, DenseShape, DenseShape, GemmProduct>
		: generic_product_impl_base<MatrixFreeOperator, Mtype, generic_product_impl<MatrixFreeOperator,Mtype>>
		{

			typedef typename Product<MatrixFreeOperator,Mtype>::Scalar Scalar;

			template<typename Dest>
			static void scaleAndAddTo(Dest& dst, const MatrixFreeOperator& op, const Mtype &m, const Scalar& alpha)
			{
				//returns dst = alpha * op * v
				// alpha must be 1 here
				assert(alpha==Scalar(1) && "scaling is not implemented");
				EIGEN_ONLY_USED_FOR_DEBUG(alpha);

				// make the mat vect product
				for (int i=0; i<op.cols();i++)
				{
					for (int j=0; j<m.cols(); j++)
						dst.col(j) += m(i,j) * op.col(i);
				}

			}
		};
	}
}

#endif


#include <iostream>
#include <chrono>
#include <Eigen/Dense>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>

#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>

using namespace std;
using namespace Eigen;

#ifdef __cplusplus
extern "C"
{
#endif
#include "calc_cnv_res_equilibrium_NaHCO3_CaCl2.h"
#ifdef __cplusplus
}
#endif
#include "eq_exported_constanst.h"

// Generic functor
template <typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
struct Functor
{
    typedef _Scalar Scalar;
    enum
    {
        InputsAtCompileTime = NX,
        ValuesAtCompileTime = NY
    };
    typedef Eigen::Matrix<Scalar, InputsAtCompileTime, 1> InputType;
    typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, 1> ValueType;
    typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;

    int m_inputs, m_values;

    Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
    Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

    int inputs() const { return m_inputs; }
    int values() const { return m_values; }
};

struct my_functor : Functor<double>
{
    my_functor(void) : Functor<double>(2, 2) {}
    int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
    {
        // Implement y = 10*(x0+3)^2 + (x1-5)^2
        fvec(0) = 10.0 * pow(x(0) + 3.0, 2) + pow(x(1) - 5.0, 2);
        fvec(1) = 0;

        return 0;
    }
};

void testStackOverflowSampleOpt()
{

    int8_t N = 15;
    double state_vals[15] = {-4.41410428, -5.986602, -7.88445742, -2.00194266, -8.30714557,
                             -2.03389701, -4.23809309, -5.21713241, -7.94810423, -3.68749322,
                             -2.34133052, -7.36356081, -3.85258587, -3.51907119, -2.001};

    Eigen::VectorXd x;
    x << -4.41410428, -5.986602, -7.88445742, -2.00194266, -8.30714557,
        -2.03389701, -4.23809309, -5.21713241, -7.94810423, -3.68749322,
        -2.34133052, -7.36356081, -3.85258587, -3.51907119, -2.001;
    // x(0) = 2.0;
    // x(1) = 3.0;
    std::cout << "x: " << x << std::endl;

    my_functor functor;
    Eigen::NumericalDiff<my_functor> numDiff(functor);
    Eigen::LevenbergMarquardt<Eigen::NumericalDiff<my_functor>, double> lm(numDiff);
    lm.parameters.maxfev = 2000;
    lm.parameters.xtol = 1.0e-10;
    std::cout << lm.parameters.maxfev << std::endl;

    int ret = lm.minimize(x);
    std::cout << lm.iter << std::endl;
    std::cout << ret << std::endl;

    std::cout << "x that minimizes the function: " << x << std::endl;

    std::cout << "press [ENTER] to continue " << std::endl;
    std::cin.get();
    return;
}

struct hybrd_functor : Functor<double>
{
    hybrd_functor(int N, const VectorXd &concs_in, double TK_in) :
        Functor<double>(N, N),
        concs(concs_in),
        TK(TK_in) {}

    int operator()(const VectorXd &x, VectorXd &fvec) const
    {
        double temp, temp1, temp2;
        const VectorXd::Index n = x.size();

        assert(fvec.size() == n);
        double *x_ptr = (double *)x.data();
        double *fvec_ptr = fvec.data();
        calc_cnv_res_equilibrium_NaHCO3_CaCl2(TK, (double *)concs.data(), x_ptr, fvec_ptr);

        return 0;
    }
    int df(const VectorXd &x, MatrixXd &fjac)
    {
        const VectorXd::Index n = x.size();
        assert(fjac.rows() == n);
        assert(fjac.cols() == n);

        double *x_ptr = (double *)x.data();
        double *J_ptr = fjac.data();
        calc_jac(TK, x_ptr, J_ptr);
        fjac.transposeInPlace();
        return 0;
    }

    private:
        VectorXd concs;
        double TK;
};

std::chrono::duration<double> getElapsedTime(const std::chrono::time_point<std::chrono::system_clock> &start)
{
    std::chrono::time_point<std::chrono::system_clock> end = std::chrono::system_clock::now();
    std::chrono::duration<double> elapsed_seconds = end - start;
    return elapsed_seconds;
}

void testHybrj1()
{
    const int n = 15;
    int info;
    VectorXd x(n);
    // x << -1, -5.986602  , -7.88445742, -2.00194266, -8.30714557,
    //     -2.03389701, -4.23809309, -5.21713241, -7.94810423, -3.68749322,
    //     -2.34133052, -7.36356081, -3.85258587, -3.51907119, -2.1;
    VectorXd concs(2);
    concs << 10e-3, 5e-3;
    double TK = 25.0 + 273.15;

    /* the following starting values provide a rough fit. */
    x.setConstant(n, -1.);

    std::cout << "Hybr: w/o Jacobian" << std::endl;
    std::chrono::time_point<std::chrono::system_clock> start = std::chrono::system_clock::now();

    hybrd_functor functor(n, concs, TK);
    HybridNonLinearSolver<hybrd_functor> solver(functor);
    info = solver.solveNumericalDiff(x);

    std::cout << x << std::endl;
    std::cout << "Solution Code: " << info << std::endl;
    std::cout << "Elapsed: " << getElapsedTime(start).count() << " s" << std::endl;

    //-----------------------------------------------------------------------
    x.setConstant(n, -1.);
    std::cout << "Hybr: w/ Jacobian" << std::endl;
    start = std::chrono::system_clock::now();

    hybrd_functor functorJac(n, concs, TK);
    HybridNonLinearSolver<hybrd_functor> solverJac(functorJac);
    info = solverJac.hybrj1(x);

    std::cout << x << std::endl;
    std::cout << "Solution Code: " << info << std::endl;
    std::cout << "Elapsed: " << getElapsedTime(start).count() << " s" << std::endl;

    //-----------------------------------------------------------------------
    std::cout << "Calculating Calcite Saturation Index:" << info << std::endl;
    double SI_calcite = calc_phase_SI_Calcite(TK, x.data());
    std::cout << "SI Calcite = " << SI_calcite << std::endl;

    VectorXd gammas(n);
    calc_gammas(TK, x.data(), gammas.data());
    std::cout << "Gammas: \n " << gammas << std::endl;
    double pH = - log10(pow(10, x[IDX_SPECIES["H+"]]) * gammas[IDX_SPECIES["H+"]]);
    std::cout << "pH = " << pH << std::endl;

    return;
}

int main(int argc, char *argv[])
{
    testHybrj1();
    return 0;
}