#include "Kinematics.hpp"

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <cmath>
#include <climits>
#include <vector>

#include <perception/vision/VisionDefs.hpp>
#include <utils/Timer.hpp>
#include <utils/Logger.hpp>
#include <utils/body.hpp>

// used with filterZeros function
static const float VERY_SMALL = 0.0001;

// These are the offsets of each of the parts in the kinematics chain, see
// http://runswift.cse.unsw.edu.au/confluence/download/attachments/3047440/100215-NaoForwardKinematicsLegToCamera.pptx.pdf?version=2&modificationDate=1266227985534
// for a visual representation of how the Foot->Camera DH chain was calculated
// for the v3s.
// For a more up to date version (v4 H21 robots) with the DH chain for the limbs
// and centre of masses, see section 3.1 of
// http://cgi.cse.unsw.edu.au/~robocup/2012site/reports/Belinda_Teh_Thesis.pdf
// Latest data for these constants can be obtained from aldebaran documentation
// http://www.aldebaran-robotics.com/documentation/family/nao_h25/links_h25.html
// and should be updated in the utils/body.hpp file.
static const float trunk_length = Limbs::HipOffsetZ + Limbs::NeckOffsetZ;

// eventually will get these static const from constants section
// TODO(dpad): actually get these from consts section
static const float camera_out_bottom = 50.71;
static const float camera_up_bottom = 17.74;

static const float camera_out_top = 58.71;
static const float camera_up_top = 63.64;

static const float camera_bottom_angle = DEG2RAD(39.7);
static const float camera_top_angle = DEG2RAD(1.2);

static const float d2 = trunk_length - Limbs::HipOffsetY;
static const float d3 = Limbs::HipOffsetY * sqrt(2);

static const float d1Top = sqrt(pow(camera_out_top, 2) + pow(camera_up_top, 2));
static const float a1Top = atan(camera_up_top / camera_out_top) + camera_top_angle;
static const float l10Top = d1Top * sin(a1Top);
static const float d11Top = d1Top * cos(a1Top);
static const float a3Top = camera_top_angle - M_PI;

static const float d1Bot = sqrt(pow(camera_out_bottom, 2) + pow(camera_up_bottom, 2));
static const float a1Bot = atan(camera_up_bottom / camera_out_bottom) + camera_bottom_angle;
static const float l10Bot = d1Bot * sin(a1Bot);
static const float d11Bot = d1Bot * cos(a1Bot);
static const float a3Bot = camera_bottom_angle - M_PI;

Kinematics::Kinematics() {
   std::vector<boost::numeric::ublas::matrix<float> > chest;

   // Left side of body
   chest.push_back(vec4<float>(-96, 100, Limbs::NeckOffsetZ - 50, 1));
   chest.push_back(vec4<float>(-96, 155, Limbs::NeckOffsetZ - 35, 1));
   chest.push_back(vec4<float>(-91, 155, Limbs::NeckOffsetZ - 34, 1));

   chest.push_back(vec4<float>(-90, 155, Limbs::NeckOffsetZ + -30, 1));
   chest.push_back(vec4<float>(-70, 155, Limbs::NeckOffsetZ + 0, 1));
   chest.push_back(vec4<float>(-50, 155, Limbs::NeckOffsetZ + 30, 1));
   chest.push_back(vec4<float>(-30, 155, Limbs::NeckOffsetZ + 30, 1));
   chest.push_back(vec4<float>(-20, 155, Limbs::NeckOffsetZ + 34, 1));
   chest.push_back(vec4<float>(-10, 155, Limbs::NeckOffsetZ + 37, 1));
   chest.push_back(vec4<float>(0, 155, Limbs::NeckOffsetZ + 40, 1));
   chest.push_back(vec4<float>(10, 155, Limbs::NeckOffsetZ + 37, 1));
   chest.push_back(vec4<float>(20, 155, Limbs::NeckOffsetZ + 34, 1));
   chest.push_back(vec4<float>(30, 155, Limbs::NeckOffsetZ + 30, 1));
   chest.push_back(vec4<float>(50, 155, Limbs::NeckOffsetZ + 30, 1));
   chest.push_back(vec4<float>(70, 155, Limbs::NeckOffsetZ + 0, 1));
   chest.push_back(vec4<float>(90, 155, Limbs::NeckOffsetZ + -30, 1));

   chest.push_back(vec4<float>(60, 70, Limbs::NeckOffsetZ - 40, 1));
   // bodyParts.push_back(chest);
   // chest.clear();

   chest.push_back(vec4<float>(60, 60, Limbs::NeckOffsetZ - 70, 1));
   chest.push_back(vec4<float>(62, 50, Limbs::NeckOffsetZ - 70, 1));
   chest.push_back(vec4<float>(63, 40, Limbs::NeckOffsetZ - 70, 1));
   chest.push_back(vec4<float>(65, 20, Limbs::NeckOffsetZ - 70, 1));
   chest.push_back(vec4<float>(65, 10, Limbs::NeckOffsetZ - 70, 1));
   chest.push_back(vec4<float>(65, 0, Limbs::NeckOffsetZ - 70, 1));
   bodyParts.push_back(chest);
   chest.clear();

   // reflection
   chest.push_back(vec4<float>(65, 0, Limbs::NeckOffsetZ - 70, 1));
   chest.push_back(vec4<float>(65, -10, Limbs::NeckOffsetZ - 70, 1));
   chest.push_back(vec4<float>(65, -20, Limbs::NeckOffsetZ - 70, 1));
   chest.push_back(vec4<float>(63, -40, Limbs::NeckOffsetZ - 70, 1));
   chest.push_back(vec4<float>(62, -50, Limbs::NeckOffsetZ - 70, 1));
   chest.push_back(vec4<float>(60, -60, Limbs::NeckOffsetZ - 70, 1));
   // bodyParts.push_back(chest);
   // chest.clear();

   // right side of body
   chest.push_back(vec4<float>(60, -70, Limbs::NeckOffsetZ - 40, 1));

   chest.push_back(vec4<float>(90, -155, Limbs::NeckOffsetZ + -30, 1));
   chest.push_back(vec4<float>(70, -155, Limbs::NeckOffsetZ + 0, 1));
   chest.push_back(vec4<float>(50, -155, Limbs::NeckOffsetZ + 10, 1));
   chest.push_back(vec4<float>(30, -155, Limbs::NeckOffsetZ + 10, 1));
   chest.push_back(vec4<float>(20, -155, Limbs::NeckOffsetZ + 14, 1));
   chest.push_back(vec4<float>(10, -155, Limbs::NeckOffsetZ + 17, 1));
   chest.push_back(vec4<float>(0, -155, Limbs::NeckOffsetZ + 20, 1));
   chest.push_back(vec4<float>(-10, -155, Limbs::NeckOffsetZ + 17, 1));
   chest.push_back(vec4<float>(-20, -155, Limbs::NeckOffsetZ + 14, 1));
   chest.push_back(vec4<float>(-30, -155, Limbs::NeckOffsetZ + 10, 1));
   chest.push_back(vec4<float>(-50, -155, Limbs::NeckOffsetZ + 10, 1));
   chest.push_back(vec4<float>(-70, -155, Limbs::NeckOffsetZ + 0, 1));
   chest.push_back(vec4<float>(-90, -155, Limbs::NeckOffsetZ + -30, 1));

   chest.push_back(vec4<float>(-91, -155, Limbs::NeckOffsetZ - 34, 1));
   chest.push_back(vec4<float>(-96, -155, Limbs::NeckOffsetZ - 35, 1));
   chest.push_back(vec4<float>(-96, -100, Limbs::NeckOffsetZ - 50, 1));
   bodyParts.push_back(chest);
   chest.clear();

   // Setting up the masses vector to store the mass of each joint in kgs.
   // These are particularly ordered in the order that they are multiplied up
   // through the kinematics chain for use in Centre of Mass calculations.
   masses.clear();
   masses.push_back(Limbs::TorsoMass); // 0 - Torso
   masses.push_back(Limbs::NeckMass); // 1 - Neck (HeadYaw)
   masses.push_back(Limbs::HeadMass); // 2 - Head (HeadPitch)

   masses.push_back(Limbs::RightShoulderMass); // 3 - Right Shoulder (RShoulderPitch)
   masses.push_back(Limbs::RightBicepMass); // 4 - Right Bicep (RShoulderRoll)
   masses.push_back(Limbs::RightElbowMass); // 5 - Right Elbow (RElbowYaw)
   masses.push_back(Limbs::RightForearmMass); // 6 - Right Motorized Forearms (RElbowRoll)
   masses.push_back(Limbs::RightHandMass); // 7 - Right Motorized Hand (RWristYaw)

   masses.push_back(Limbs::LeftShoulderMass); // 8 - Left Shoulder (LShoulderPitch)
   masses.push_back(Limbs::LeftBicepMass); // 9 - Left Bicep (LShoulderRoll)
   masses.push_back(Limbs::LeftElbowMass); // 10 - Left Elbow (LElbowYaw)
   masses.push_back(Limbs::LeftForearmMass); // 11 - Left Motorized Forearm (LElboRoll)
   masses.push_back(Limbs::LeftHandMass); // 12 - Left Motorized Hand (LWristYaw)

   masses.push_back(Limbs::RightPelvisMass); // 13 - Right Pelvis (RHipYawPitch)
   masses.push_back(Limbs::RightHipMass); // 14 - Right Hip (RHipRoll)
   masses.push_back(Limbs::RightThighMass); // 15 - Right Thigh (RHipPitch)
   masses.push_back(Limbs::RightTibiaMass); // 16 - Right Tibia (RKneePitch)
   masses.push_back(Limbs::RightAnkleMass); // 17 - Right Ankle (RAnklePitch)
   masses.push_back(Limbs::RightFootMass); // 18 - Right Foot (RAnkleRoll)

   masses.push_back(Limbs::LeftPelvisMass); // 19 - Left Pelvis (LHipYawPitch)
   masses.push_back(Limbs::LeftHipMass); // 20 - Left Hip (LHipRoll)
   masses.push_back(Limbs::LeftThighMass); // 21 - Left Thigh (LHipPitch)
   masses.push_back(Limbs::LeftTibiaMass); // 22 - Left Tibia (LKneePitch)
   masses.push_back(Limbs::LeftAnkleMass); // 23 - Left Ankle (LAnklePitch)
   masses.push_back(Limbs::LeftFootMass); // 24 - Left Foot (LAnkleRoll)

   // Same order as above, but this time for the position of the centre of mass for each joint.
   massesCom.clear();
   massesCom.push_back(vec4(Limbs::TorsoCoM)); // Torso
   massesCom.push_back(vec4(Limbs::NeckCoM)); // Head
   massesCom.push_back(vec4(Limbs::HeadCoM));
   massesCom.push_back(vec4(Limbs::RightShoulderCoM)); // R Arm
   massesCom.push_back(vec4(Limbs::RightBicepCoM));
   massesCom.push_back(vec4(Limbs::RightElbowCoM));
   massesCom.push_back(vec4(Limbs::RightForearmCoM));
   massesCom.push_back(vec4(Limbs::RightHandCoM));
   massesCom.push_back(vec4(Limbs::LeftShoulderCoM)); // L Arm
   massesCom.push_back(vec4(Limbs::LeftBicepCoM));
   massesCom.push_back(vec4(Limbs::LeftElbowCoM));
   massesCom.push_back(vec4(Limbs::LeftForearmCoM));
   massesCom.push_back(vec4(Limbs::LeftHandCoM));
   massesCom.push_back(vec4(Limbs::RightPelvisCoM)); // R Leg
   massesCom.push_back(vec4(Limbs::RightHipCoM));
   massesCom.push_back(vec4(Limbs::RightThighCoM));
   massesCom.push_back(vec4(Limbs::RightTibiaCoM));
   massesCom.push_back(vec4(Limbs::RightAnkleCoM));
   massesCom.push_back(vec4(Limbs::RightFootCoM));
   massesCom.push_back(vec4(Limbs::LeftPelvisCoM)); // L Leg
   massesCom.push_back(vec4(Limbs::LeftHipCoM));
   massesCom.push_back(vec4(Limbs::LeftThighCoM));
   massesCom.push_back(vec4(Limbs::LeftTibiaCoM));
   massesCom.push_back(vec4(Limbs::LeftAnkleCoM));
   massesCom.push_back(vec4(Limbs::LeftFootCoM));

   // Initialise transform matrices for DH chain
   for (int i = 0; i < CAMERA_DH_CHAIN_LEN; ++i) {
      transformLTop[i] = boost::numeric::ublas::identity_matrix<float>(4);
      transformRTop[i] = boost::numeric::ublas::identity_matrix<float>(4);
      transformLBot[i] = boost::numeric::ublas::identity_matrix<float>(4);
      transformRBot[i] = boost::numeric::ublas::identity_matrix<float>(4);
   }
   for (int i = 0; i < HEAD_DH_CHAIN_LEN; ++i) {
      transformHB[i] = boost::numeric::ublas::identity_matrix<float>(4);
   }
   for (int i = 0; i < ARM_DH_CHAIN_LEN; ++i) {
      transformLAB[i] = boost::numeric::ublas::identity_matrix<float>(4);
      transformRAB[i] = boost::numeric::ublas::identity_matrix<float>(4);
   }
   for (int i = 0; i < LEG_DH_CHAIN_LEN; ++i) {
      transformLFB[i] = boost::numeric::ublas::identity_matrix<float>(4);
      transformRFB[i] = boost::numeric::ublas::identity_matrix<float>(4);
   }

   // Set up constant DH transforms since they only need to be calculated once
   boost::numeric::ublas::matrix<float> pi2AboutX = createDHMatrix<float>(0, M_PI / 2, 0, 0);
   boost::numeric::ublas::matrix<float> negpi2AboutX = createDHMatrix<float>(0, -M_PI / 2, 0, 0);
   boost::numeric::ublas::matrix<float> pi4AboutX = createDHMatrix<float>(0, M_PI / 4, 0, 0);
   boost::numeric::ublas::matrix<float> negpi2AboutXnegpi2AboutZ = createDHMatrix<float>(0, -M_PI / 2, 0, -M_PI / 2);
   boost::numeric::ublas::matrix<float> pi2AboutXpi2AboutZ = createDHMatrix<float>(0, M_PI / 2, 0, M_PI / 2);

   // Constants in camera transforms
   transformLTop[0] = createDHMatrix<float>(0, 0, Limbs::FootHeight, M_PI / 2);
   transformLTop[7] = createDHMatrix<float>(0, 3 * M_PI / 4, 0, 0);

   transformRTop[0] = transformLTop[0];
   transformRTop[7] = pi4AboutX;

   transformLBot[0] = transformLTop[0];
   transformLBot[7] = transformLTop[7];

   transformRBot[0] = transformRTop[0];
   transformRBot[7] = transformRTop[7];

   // Constants in mass transforms
   transformHB[2] = pi2AboutX;

   transformRAB[0] = createDHMatrix<float>(0, 0, Limbs::ShoulderOffsetZ, 0);
   transformRAB[1] = createDHMatrix<float>(0, M_PI / 2, Limbs::ShoulderOffsetY, 0);
   transformRAB[3] = pi2AboutX;
   transformRAB[5] = createDHMatrix<float>(Limbs::UpperArmLength, M_PI / 2,
                                           Limbs::ElbowOffsetY, M_PI / 2);
   transformRAB[7] = negpi2AboutXnegpi2AboutZ;
   transformRAB[8] = negpi2AboutX;
   transformRAB[10] = createDHMatrix<float>(Limbs::LowerArmLength, M_PI / 2, 0, M_PI / 2);
   transformRAB[12] = negpi2AboutXnegpi2AboutZ;
   transformRAB[13] = negpi2AboutX;

   transformLAB[0] = transformRAB[0];
   transformLAB[1] = createDHMatrix<float>(0, M_PI / 2, -Limbs::ShoulderOffsetY, 0);
   transformLAB[3] = transformRAB[3];
   transformLAB[5] = createDHMatrix<float>(Limbs::UpperArmLength, M_PI / 2,
                                          -Limbs::ElbowOffsetY, M_PI / 2);
   transformLAB[7] = transformRAB[7];
   transformLAB[8] = transformRAB[8];
   transformLAB[10] = transformRAB[10];
   transformLAB[12] = transformRAB[12];
   transformLAB[13] = transformRAB[13];

   transformRFB[0] = createDHMatrix<float>(0, 0, -Limbs::HipOffsetZ, 0);
   transformRFB[1] = createDHMatrix<float>(0, M_PI / 2, Limbs::HipOffsetY, 0);
   transformRFB[3] = pi4AboutX;
   transformRFB[4] = createDHMatrix<float>(0, M_PI / 2, 0, -M_PI / 2);
   transformRFB[6] = pi2AboutXpi2AboutZ;
   transformRFB[7] = negpi2AboutX;
   transformRFB[9] = pi2AboutX;
   transformRFB[10] = createDHMatrix<float>(0, 0, -Limbs::ThighLength, 0);
   transformRFB[12] = pi2AboutX;
   transformRFB[13] = createDHMatrix<float>(0, 0, -Limbs::TibiaLength, 0);
   transformRFB[15] = pi2AboutX;
   transformRFB[16] = createDHMatrix<float>(0, 0, 0, -M_PI / 2);
   transformRFB[18] = pi2AboutXpi2AboutZ;

   transformLFB[0] = transformRFB[0];
   transformLFB[1] = createDHMatrix<float>(0, M_PI / 2, -Limbs::HipOffsetY, 0);
   transformLFB[3] = createDHMatrix<float>(0, -M_PI / 4, 0, 0);
   transformLFB[4] = transformRFB[4];
   transformLFB[6] = transformRFB[6];
   transformLFB[7] = transformRFB[7];
   transformLFB[9] = transformRFB[9];
   transformLFB[10] = transformRFB[10];
   transformLFB[12] = transformRFB[12];
   transformLFB[13] = transformRFB[13];
   transformLFB[15] = transformRFB[15];
   transformLFB[16] = transformRFB[16];
   transformLFB[18] = transformRFB[18];

}

Kinematics::Chain
Kinematics::determineSupportChain() {
   float lsum = sensorValues.sensors[Sensors::LFoot_FSR_FrontLeft]  +
                sensorValues.sensors[Sensors::LFoot_FSR_FrontRight] +
                sensorValues.sensors[Sensors::LFoot_FSR_RearLeft]   +
                sensorValues.sensors[Sensors::LFoot_FSR_RearRight];
   float rsum = sensorValues.sensors[Sensors::RFoot_FSR_FrontLeft]  +
                sensorValues.sensors[Sensors::RFoot_FSR_FrontRight] +
                sensorValues.sensors[Sensors::RFoot_FSR_RearLeft]   +
                sensorValues.sensors[Sensors::RFoot_FSR_RearRight];
   if (lsum > rsum) return LEFT_CHAIN;
   return RIGHT_CHAIN;
}

void Kinematics::updateDHChain() {
   // These are the camera offsets from kinematics calibrations
   float coffsetY, coffsetX, coffsetZ;

   JointValues jointValues = sensorValues.joints;

   // Calculate the top left camera transform
   coffsetY = DEG2RAD(parameters.cameraPitchTop);
   coffsetX = DEG2RAD(parameters.cameraYawTop);
   coffsetZ = DEG2RAD(parameters.cameraRollTop);

   float Cp = jointValues.angles[Joints::HeadPitch];
   float Cy = jointValues.angles[Joints::HeadYaw];
   float Hyp = jointValues.angles[Joints::LHipYawPitch];
   float HpL = jointValues.angles[Joints::LHipPitch];
   float HrL = jointValues.angles[Joints::LHipRoll];
   float KpL = jointValues.angles[Joints::LKneePitch];
   float ApL = jointValues.angles[Joints::LAnklePitch];
   float ArL = jointValues.angles[Joints::LAnkleRoll];

   cameraPanInverseHack = createDHMatrix<float>(0, 0, d2, 0.0);
   // DH parameters
   // Some of these are commented out since they're constant and can be calculated
   // once at the start
   //transformLTop[0] = createDHMatrix<float>(0, 0, Limbs::FootHeight, M_PI / 2);
   transformLTop[1] = createDHMatrix<float>(0, M_PI / 2, 0, M_PI / 2 - ArL);
   transformLTop[2] = createDHMatrix<float>(0, M_PI / 2, 0, -ApL);
   transformLTop[3] = createDHMatrix<float>(Limbs::TibiaLength, 0, 0, -KpL);
   transformLTop[4] = createDHMatrix<float>(Limbs::ThighLength, 0, 0, -HpL);
   transformLTop[5] = createDHMatrix<float>(0, -M_PI / 2, 0, -M_PI / 4 - HrL);
   transformLTop[6] = createDHMatrix<float>(0, M_PI / 2, -d3, M_PI / 2 - Hyp);
   // transformLTop[7] = createDHMatrix<float>(0, 3 * M_PI / 4, 0, 0);
   transformLTop[8] = createDHMatrix<float>(0, 0, d2, Cy);
   transformLTop[9] = createDHMatrix<float>(0, -M_PI / 2, 0, a3Top + Cp);
   transformLTop[10] = createDHMatrix<float>(0, -M_PI/2, l10Top, M_PI / 2 + coffsetX);
   transformLTop[11] = createDHMatrix<float>(0, -M_PI/2 + coffsetY, d11Top, coffsetZ);

   // Calculate the top right camera transform
   float HpR = jointValues.angles[Joints::RHipPitch];
   float HrR = jointValues.angles[Joints::RHipRoll];
   float KpR = jointValues.angles[Joints::RKneePitch];
   float ApR = jointValues.angles[Joints::RAnklePitch];
   float ArR = jointValues.angles[Joints::RAnkleRoll];

   // DH parameters
   // Some of these are commented out since they're constant and can be calculated once
   // Just leaving them here so they can be seen in order
   //transformRTop[0] = transformLTop[0];
   transformRTop[1] = createDHMatrix<float>(0, M_PI / 2, 0, M_PI / 2 - ArR);
   transformRTop[2] = createDHMatrix<float>(0, M_PI / 2, 0, -ApR);
   transformRTop[3] = createDHMatrix<float>(Limbs::TibiaLength, 0, 0, -KpR);
   transformRTop[4] = createDHMatrix<float>(Limbs::ThighLength, 0, 0, -HpR);
   transformRTop[5] = createDHMatrix<float>(0, -M_PI / 2, 0, M_PI / 4 - HrR);
   transformRTop[6] = createDHMatrix<float>(0, M_PI / 2, d3, M_PI / 2 - Hyp);
   //transformRTop[7] = createDHMatrix<float>(0, M_PI / 4, 0, 0);
   transformRTop[8] = transformLTop[8];
   transformRTop[9] = transformLTop[9];
   transformRTop[10] = transformLTop[10];
   transformRTop[11] = transformLTop[11];

   // Calculate the bottom left camera transform
   coffsetY = DEG2RAD(parameters.cameraPitchBottom);
   coffsetX = DEG2RAD(parameters.cameraYawBottom);
   coffsetZ = DEG2RAD(parameters.cameraRollBottom);

   //transformLBot[0] = transformLTop[0];
   transformLBot[1] = transformLTop[1];
   transformLBot[2] = transformLTop[2];
   transformLBot[3] = transformLTop[3];
   transformLBot[4] = transformLTop[4];
   transformLBot[5] = transformLTop[5];
   transformLBot[6] = transformLTop[6];
   //transformLBot[7] = transformLTop[7];
   transformLBot[8] = transformLTop[8];
   transformLBot[9] = createDHMatrix<float>(0, -M_PI / 2, 0, a3Bot + Cp);
   transformLBot[10] = createDHMatrix<float>(0, -M_PI/2, l10Bot, M_PI / 2 + coffsetX);
   transformLBot[11] = createDHMatrix<float>(0, -M_PI/2 + coffsetY, d11Bot, coffsetZ);

   // Calculate the bottom right transform
   //transformRBot[0] = transformRTop[0];
   transformRBot[1] = transformRTop[1];
   transformRBot[2] = transformRTop[2];
   transformRBot[3] = transformRTop[3];
   transformRBot[4] = transformRTop[4];
   transformRBot[5] = transformRTop[5];
   transformRBot[6] = transformRTop[6];
   //transformRBot[7] = transformRTop[7];
   transformRBot[8] = transformRTop[8];
   transformRBot[9] = transformLBot[9];
   transformRBot[10] = transformLBot[10];
   transformRBot[11] = transformLBot[11];

   // Transform parameters for centre of mass
   // Head to Body
   transformHB[0] = createDHMatrix<float>(0, 0, Limbs::NeckOffsetZ, Cy);
   transformHB[1] = createDHMatrix<float>(0, -M_PI / 2, 0, Cp);
   //transformHB[2] = createDHMatrix<float>(0, M_PI / 2, 0, 0);

   // Right Arm to Body
   // Some of these are commented out since they're constant and can be calculated once
   // Just leaving them here so they can be seen in order
   float Sp = jointValues.angles[Joints::RShoulderPitch];
   float Sr = jointValues.angles[Joints::RShoulderRoll];
   float Ey = jointValues.angles[Joints::RElbowYaw];
   float Er = jointValues.angles[Joints::RElbowRoll];
   float Wy = jointValues.angles[Joints::RWristYaw];
   //transformRAB[0] = createDHMatrix<float>(0, 0, Limbs::ShoulderOffsetZ, 0);
   //transformRAB[1] = createDHMatrix<float>(0, M_PI / 2, Limbs::ShoulderOffsetY, 0);
   transformRAB[2] = createDHMatrix<float>(0, -M_PI, 0, Sp);
   //transformRAB[3] = createDHMatrix<float>(0, M_PI / 2, 0, 0);
   transformRAB[4] = createDHMatrix<float>(0, 0, 0, Sr);
   //transformRAB[5] = createDHMatrix<float>(Limbs::UpperArmLength, M_PI / 2,
   //                                        Limbs::ElbowOffsetY, M_PI / 2);
   transformRAB[6] = createDHMatrix<float>(0, M_PI / 2, 0, Ey);
   //transformRAB[7] = createDHMatrix<float>(0, -M_PI / 2, 0, -M_PI / 2);
   //transformRAB[8] = createDHMatrix<float>(0, -M_PI / 2, 0, 0);
   transformRAB[9] = createDHMatrix<float>(0, 0, 0, Er);
   //transformRAB[10] = createDHMatrix<float>(Limbs::LowerArmLength, M_PI / 2, 0, M_PI / 2);
   transformRAB[11] = createDHMatrix<float>(0, M_PI / 2, 0, Wy);
   //transformRAB[12] = createDHMatrix<float>(0, -M_PI / 2, 0, -M_PI / 2);
   //transformRAB[13] = createDHMatrix<float>(0, -M_PI / 2, 0, 0);

   // Left Arm to Body
   Sp = jointValues.angles[Joints::LShoulderPitch];
   Sr = jointValues.angles[Joints::LShoulderRoll];
   Ey = jointValues.angles[Joints::LElbowYaw];
   Er = jointValues.angles[Joints::LElbowRoll];
   Wy = jointValues.angles[Joints::LWristYaw];
   //transformLAB[0] = transformRAB[0];
   //transformLAB[1] = createDHMatrix<float>(0, M_PI / 2, -Limbs::ShoulderOffsetY, 0);
   transformLAB[2] = createDHMatrix<float>(0, -M_PI, 0, Sp);
   //transformLAB[3] = transformRAB[3];
   transformLAB[4] = createDHMatrix<float>(0, 0, 0, Sr);
   //transformRAB[5] = createDHMatrix<float>(Limbs::UpperArmLength, M_PI / 2,
   //                                       -Limbs::ElbowOffsetY, M_PI / 2);
   transformLAB[6] = createDHMatrix<float>(0, M_PI / 2, 0, Ey);
   //transformLAB[7] = transformRAB[7];
   //transformLAB[8] = transformRAB[8];
   transformLAB[9] = createDHMatrix<float>(0, 0, 0, Er);
   //transformLAB[10] = transformRAB[10];
   transformLAB[11] = createDHMatrix<float>(0, M_PI / 2, 0, Wy);
   //transformLAB[12] = transformRAB[12];
   //transformLAB[13] = transformRAB[13];

   // Right Foot to Body
   HpR = jointValues.angles[Joints::RHipPitch];
   HrR = jointValues.angles[Joints::RHipRoll];
   //transformRFB[0] = createDHMatrix<float>(0, 0, -Limbs::HipOffsetZ, 0);
   //transformRFB[1] = createDHMatrix<float>(0, M_PI / 2, Limbs::HipOffsetY, 0);
   transformRFB[2] = createDHMatrix<float>(0, -3 * M_PI / 4, 0, Hyp);
   //transformRFB[3] = createDHMatrix<float>(0, M_PI / 4, 0, 0);
   //transformRFB[4] = createDHMatrix<float>(0, M_PI / 2, 0, -M_PI / 2);
   transformRFB[5] = createDHMatrix<float>(0, -M_PI / 2, 0, HrR);
   //transformRFB[6] = createDHMatrix<float>(0, M_PI / 2, 0, M_PI / 2);
   //transformRFB[7] = createDHMatrix<float>(0, -M_PI / 2, 0, 0);
   transformRFB[8] = createDHMatrix<float>(0, -M_PI / 2, 0, HpR);
   //transformRFB[9] = createDHMatrix<float>(0, M_PI / 2, 0, 0);
   //transformRFB[10] = createDHMatrix<float>(0, 0, -Limbs::ThighLength, 0);
   transformRFB[11] = createDHMatrix<float>(0, -M_PI / 2, 0, KpR);
   //transformRFB[12] = createDHMatrix<float>(0, M_PI / 2, 0, 0);
   //transformRFB[13] = createDHMatrix<float>(0, 0, -Limbs::TibiaLength, 0);
   transformRFB[14] = createDHMatrix<float>(0, -M_PI / 2, 0, ApR);
   //transformRFB[15] = createDHMatrix<float>(0, M_PI / 2, 0, 0);
   //transformRFB[16] = createDHMatrix<float>(0, 0, 0, -M_PI / 2);
   transformRFB[17] = createDHMatrix<float>(0, -M_PI / 2, 0, ArR);
   //transformRFB[18] = createDHMatrix<float>(0, M_PI / 2, 0, M_PI / 2);

   // Left Foot to Body
   HpL = jointValues.angles[Joints::LHipPitch];
   HrL = jointValues.angles[Joints::LHipRoll];
   //transformLFB[0] = transformRFB[0];
   //transformLFB[1] = createDHMatrix<float>(0, M_PI / 2, -Limbs::HipOffsetY, 0);
   transformLFB[2] = createDHMatrix<float>(0, -M_PI / 4, 0, -Hyp);
   //transformLFB[3] = createDHMatrix<float>(0, -M_PI / 4, 0, 0);
   //transformLFB[4] = transformRFB[4];
   transformLFB[5] = createDHMatrix<float>(0, -M_PI / 2, 0, HrL);
   //transformLFB[6] = transformRFB[6];
   //transformLFB[7] = transformRFB[7];
   transformLFB[8] = createDHMatrix<float>(0, -M_PI / 2, 0, HpL);
   //transformLFB[9] = transformRFB[9];
   //transformLFB[10] = transformRFB[10];
   transformLFB[11] = createDHMatrix<float>(0, -M_PI / 2, 0, KpL);
   //transformLFB[12] = transformRFB[12];
   //transformLFB[13] = transformRFB[13];
   transformLFB[14] = createDHMatrix<float>(0, -M_PI / 2, 0, ApL);
   //transformLFB[15] = transformRFB[15];
   //transformLFB[16] = transformRFB[16];
   transformLFB[17] = createDHMatrix<float>(0, -M_PI / 2, 0, ArL);
   //transformLFB[18] = transformRFB[18];
}

Pose Kinematics::getPose() {
   Chain foot = determineSupportChain();

   boost::numeric::ublas::matrix<float> c2wTop = createCameraToWorldTransform(foot, true);
   boost::numeric::ublas::matrix<float> c2wBot = createCameraToWorldTransform(foot, false);
   boost::numeric::ublas::matrix<float> n2w = createNeckToWorldTransform(foot);
   std::pair<int, int> horizon = calculateHorizon(c2wTop);
   Pose pose(c2wTop, c2wBot, n2w, horizon);

   boost::numeric::ublas::matrix<float> b2cTop =
      evaluateDHChain(BODY, CAMERA, foot, true);
   determineBodyExclusionArray(b2cTop, pose.getTopExclusionArray(), true);

   boost::numeric::ublas::matrix<float> b2cBot =
      evaluateDHChain(BODY, CAMERA, foot, false);
   determineBodyExclusionArray(b2cBot, pose.getBotExclusionArray(), false);

   return pose;
}

boost::numeric::ublas::matrix<float>
Kinematics::createCameraToWorldTransform(Chain foot, bool top) {
   boost::numeric::ublas::matrix<float> c2f = createCameraToFootTransform(foot,top);
   boost::numeric::ublas::matrix<float> f2w = createFootToWorldTransform(foot,top);
   return prod(f2w, c2f);
}

boost::numeric::ublas::matrix<float>
Kinematics::createNeckToWorldTransform(Chain foot) {
   boost::numeric::ublas::matrix<float> n2f = createNeckToFootTransform(foot);
   boost::numeric::ublas::matrix<float> f2w = createFootToWorldTransform(foot);
   return prod(f2w, n2f);
}

boost::numeric::ublas::matrix<float>
Kinematics::evaluateDHChain(Link from, Link to, Chain foot, bool top) {
   boost::numeric::ublas::matrix<float> finalTransform =
      boost::numeric::ublas::identity_matrix<float>(4);
   if (foot == RIGHT_CHAIN) {
      if (top) {
         for (int i = from; i < to; i++) {
            finalTransform = boost::numeric::ublas::prod(finalTransform,
                                                         transformRTop[i]);
         }
      } else {
         for (int i = from; i < to; i++) {
            finalTransform = boost::numeric::ublas::prod(finalTransform,
                                                         transformRBot[i]);
         }
      }
   } else {
      if (top) {
         for (int i = from; i < to; i++) {
            finalTransform =
               boost::numeric::ublas::prod(finalTransform, transformLTop[i]);
         }
      } else {
         for (int i = from; i < to; i++) {
            finalTransform =
               boost::numeric::ublas::prod(finalTransform, transformLBot[i]);
         }
      }
   }
   return finalTransform;
}

// Evaluate kinematics chain from all limbs back to the IMU, taking into account the COM at each part.
boost::numeric::ublas::matrix<float>
Kinematics::evaluateMassChain() {
   int i, joint = 0;
   float totalMass = 0;
   boost::numeric::ublas::matrix<float> finalTransform(4, 1);
   finalTransform = boost::numeric::ublas::zero_matrix<float>(4, 1);

   // Mass of torso
   finalTransform += massesCom[joint] * masses[joint];
   totalMass += masses[joint];
   ++joint;

   // Mass of head
   boost::numeric::ublas::matrix<float> headTransform = boost::numeric::ublas::identity_matrix<float>(4);
   for (i = 0; i < HEAD_DH_CHAIN_LEN; ++i) {
      headTransform = boost::numeric::ublas::prod(headTransform, transformHB[i]);
      // Up to head yaw, head pitch
      if (i == 0 || i == 2) {
         finalTransform += boost::numeric::ublas::prod(headTransform, massesCom[joint]) * masses[joint];
         totalMass += masses[joint];
         ++joint;
      }
   }

   // Mass of right arm
   boost::numeric::ublas::matrix<float> rArmTransform = boost::numeric::ublas::identity_matrix<float>(4);
   for (i = 0; i < ARM_DH_CHAIN_LEN; ++i) {
      rArmTransform = boost::numeric::ublas::prod(rArmTransform, transformRAB[i]);
      // Up to shoulder pitch, shoulder roll, elbow yaw, elbow roll, wrist yaw
      if (i == 3 || i == 4 || i == 8 || i == 9 || i == 13) {
         finalTransform += boost::numeric::ublas::prod(rArmTransform, massesCom[joint]) * masses[joint];
         totalMass += masses[joint];
         ++joint;
      }
   }

   // Mass of left arm
   boost::numeric::ublas::matrix<float> lArmTransform = boost::numeric::ublas::identity_matrix<float>(4);
   for (i = 0; i < ARM_DH_CHAIN_LEN; ++i) {
      // Up to shoulder pitch, shoulder roll, elbow yaw, elbow roll, wrist yaw
      lArmTransform = boost::numeric::ublas::prod(lArmTransform, transformLAB[i]);
      if (i == 3 || i == 4 || i == 8 || i == 9 || i == 13) {
         finalTransform += boost::numeric::ublas::prod(lArmTransform, massesCom[joint]) * masses[joint];
         totalMass += masses[joint];
         ++joint;
      }
   }

   // Mass of right leg
   boost::numeric::ublas::matrix<float> rLegTransform = boost::numeric::ublas::identity_matrix<float>(4);
   for (i = 0; i < LEG_DH_CHAIN_LEN; ++i) {
      rLegTransform = boost::numeric::ublas::prod(rLegTransform, transformRFB[i]);
      // Up to hip yaw pitch, hip roll, hip pitch, knee pitch, ankle pitch, ankle roll
      if (i == 3 || i == 7 || i == 9 || i == 12 || i == 15 || i == 18) {
         finalTransform += boost::numeric::ublas::prod(rLegTransform, massesCom[joint]) * masses[joint];
         totalMass += masses[joint];
         ++joint;
      }
   }

   // Mass of left leg
   boost::numeric::ublas::matrix<float> lLegTransform = boost::numeric::ublas::identity_matrix<float>(4);
   for (i = 0; i < LEG_DH_CHAIN_LEN; ++i) {
      lLegTransform = boost::numeric::ublas::prod(lLegTransform, transformLFB[i]);
      // Up to hip yaw pitch, hip roll, hip pitch, knee pitch, ankle pitch, ankle roll
      if (i == 3 || i == 7 || i == 9 || i == 12 || i == 15 || i == 18) {
         finalTransform += boost::numeric::ublas::prod(lLegTransform, massesCom[joint]) * masses[joint];
         totalMass += masses[joint];
         ++joint;
      }
   }

   return finalTransform / totalMass;
}

boost::numeric::ublas::matrix<float>
Kinematics::createCameraToFootTransform(Chain foot, bool top) {
   boost::numeric::ublas::matrix<float> b2f = evaluateDHChain(FOOT, BODY, foot, top);
   // When we get to the torso, we need to adjust the transform by the forward and side lean to account
   // for when the robot's feet are not flat on the ground. We apply the the rotation of the lean to the
   // hip vector to account for the lean already introduced by the leg joints.
   // We also adjust by the body pitch offset from kinematics calibration.
   boost::numeric::ublas::matrix<float> z(4, 1);
   z(0, 0) = 0;
   z(1, 0) = 0;
   z(2, 0) = 0;
   z(3, 0) = 1;
   boost::numeric::ublas::matrix<float> hipPt = prod(b2f, z);
   float bodyPitchOffset = DEG2RAD(parameters.bodyPitch);
   float forwardLean = sensorValues.sensors[Sensors::InertialSensor_AngleY];
   float sideLean = sensorValues.sensors[Sensors::InertialSensor_AngleX];

   boost::numeric::ublas::matrix<float> transform = createDHMatrix<float>(hipPt(0, 0), 0, 0, 0);
   transform = prod(transform, createDHMatrix<float>(0, 0, hipPt(2, 0), M_PI / 2)); // move up by hip height
   transform = prod(transform, createDHMatrix<float>(hipPt(1, 0), 0, 0, 0)); // move sideways
   transform = prod(transform, createDHMatrix<float>(0, forwardLean + bodyPitchOffset, 0, -M_PI / 2));
   transform = prod(transform, createDHMatrix<float>(0, sideLean, 0, 0));

   return prod(transform, evaluateDHChain(BODY, CAMERA, foot, top));
}

boost::numeric::ublas::matrix<float>
Kinematics::createNeckToFootTransform(Chain foot) {
   boost::numeric::ublas::matrix<float> b2f = evaluateDHChain(FOOT, BODY, foot);
   // When we get to the torso, we need to adjust the transform by the forward and side lean to account
   // for when the robot's feet are not flat on the ground. We apply the the rotation of the lean to the
   // hip vector to account for the lean already introduced by the leg joints.
   // We also adjust by the body pitch offset from kinematics calibration.
   boost::numeric::ublas::matrix<float> z(4, 1);
   z(0, 0) = 0;
   z(1, 0) = 0;
   z(2, 0) = 0;
   z(3, 0) = 1;
   boost::numeric::ublas::matrix<float> hipPt = prod(b2f, z);
   float bodyPitchOffset = DEG2RAD(parameters.bodyPitch);
   float forwardLean = sensorValues.sensors[Sensors::InertialSensor_AngleY];
   float sideLean = sensorValues.sensors[Sensors::InertialSensor_AngleX];

   boost::numeric::ublas::matrix<float> transform = createDHMatrix<float>(hipPt(0, 0), 0, 0, 0);
   transform = prod(transform, createDHMatrix<float>(0, 0, hipPt(2, 0), M_PI / 2)); // move up by hip height
   transform = prod(transform, createDHMatrix<float>(hipPt(1, 0), 0, 0, 0)); // move sideways
   transform = prod(transform, createDHMatrix<float>(0, forwardLean + bodyPitchOffset, 0, -M_PI / 2));
   transform = prod(transform, createDHMatrix<float>(0, sideLean, 0, 0));

   return prod(transform, cameraPanInverseHack);
}

// World is defined as the centre of the two feet on the ground plane,
// with a heading equal to the average of the two feet directions
boost::numeric::ublas::matrix<float>
Kinematics::createFootToWorldTransform(Chain foot, bool top) {
   boost::numeric::ublas::matrix<float> b2lf =
      evaluateDHChain(FOOT, BODY, foot, top);
   boost::numeric::ublas::matrix<float> b2rf =
      evaluateDHChain(FOOT, BODY, (Chain) !foot, top);

   boost::numeric::ublas::matrix<float> rf2b(4, 4);
   invertMatrix(b2rf, rf2b);

   boost::numeric::ublas::matrix<float> rf2lf(4, 4);
   rf2lf = prod(rf2b, b2lf);

   boost::numeric::ublas::matrix<float> z(4, 1);
   z(0, 0) = 0;
   z(1, 0) = 0;
   z(2, 0) = 0;
   z(3, 0) = 1;
   boost::numeric::ublas::matrix<float> forward(4, 1);
   forward(0, 0) = 1;
   forward(1, 0) = 0;
   forward(2, 0) = 0;
   forward(3, 0) = 1;

   // first find position of centre of two feet on the ground.
   z = prod(rf2lf, z);

   // find direction of second foot in first foot coords
   forward = prod(rf2lf, forward) - z;

   boost::numeric::ublas::matrix<float> position = -z / 2;
   position(3, 0) = 1;
   position(2, 0) = 0;  // on the ground

   boost::numeric::ublas::matrix<float> result =
      boost::numeric::ublas::identity_matrix<float>(4);
   result = prod(translateMatrix<float>(position(0, 0), position(1, 0), 0),
                 result);
   // result = prod(rotateZMatrix<float>(-atan2(forward(1, 0), forward(0, 0))/2.0),
   //                            result);
   result = prod(rotateZMatrix<float>(-atan2f(forward(1, 0), forward(0, 0)) / 2.0),
                                      result);
   return result;
}

void Kinematics::setSensorValues(SensorValues sensorValues) {
   this->sensorValues = sensorValues;
}

boost::numeric::ublas::matrix<float>
Kinematics::createWorldToFOVTransform(
   const boost::numeric::ublas::matrix<float> &c2w) {
   boost::numeric::ublas::matrix<float> w2c = c2w;

   invertMatrix(c2w, w2c);

   float ex = 0;
   float ey = 0;
   float ez = 1.0 / tan(IMAGE_HFOV / 2);

   boost::numeric::ublas::matrix<float> projection = projectionMatrix(ex, ey, ez);
   boost::numeric::ublas::matrix<float> transform =
      boost::numeric::ublas::prod(projection, w2c);

   return transform;
}

void Kinematics::determineBodyExclusionArray(
   const boost::numeric::ublas::matrix<float> &m,
   int16_t *points, bool top) {

   const int COLS = (top) ? TOP_IMAGE_COLS : BOT_IMAGE_COLS;

   for (int i = 0; i < Pose::EXCLUSION_RESOLUTION; i++) {
      points[i] = TOP_IMAGE_ROWS;
      if (!top) points[i] += BOT_IMAGE_ROWS;
   }
   // pixel off screen really low
   boost::numeric::ublas::matrix<float> transform
      = createWorldToFOVTransform(m);

   for (unsigned int part = 0; part < bodyParts.size(); part++) {
      boost::numeric::ublas::matrix<float> last =
         fovToImageSpaceTransform(transform,
                                  bodyParts[part][0],
                                  top);
      for (unsigned int i = 0; i < bodyParts[part].size(); i++) {
         boost::numeric::ublas::matrix<float> m
            = fovToImageSpaceTransform(transform,
                                       bodyParts[part][i],
                                       top);
         if (m(2, 0) <= 0) {
            last = m;
            continue;
         }
         // llog(VERBOSE) << "Pixel " << i << ": " << m << std::endl;
         // llog(VERBOSE) << "Coord " << i << ": " <<
         //            prod(transform, bodyParts[part][i]) << std::endl;
         int lIndex = (int)(last(0, 0) / COLS *
                            Pose::EXCLUSION_RESOLUTION);
         int cIndex = (int)(m(0, 0) / COLS *
                            Pose::EXCLUSION_RESOLUTION);
         int lPixel = last(1, 0);
         int cPixel = m(1, 0);
         // int range = ABS(cIndex - lIndex);
         float gradient = 0;
         if (cIndex - lIndex != 0) {
            float denom = cIndex - lIndex;
            gradient = (cPixel - lPixel) / (denom);
         }
         cIndex = MIN(MAX(cIndex, 0), (int) Pose::EXCLUSION_RESOLUTION);
         lIndex = MIN(MAX(lIndex, 0), (int) Pose::EXCLUSION_RESOLUTION);
         int index = lIndex;
         while (index != cIndex && last(2, 0) > 0) {
            if (index >= 0 && index < Pose::EXCLUSION_RESOLUTION &&
                index != cIndex) {
               int nPixel = last(1, 0) + gradient * (index - lIndex);
               if (nPixel < points[index]) points[index] = nPixel;
            }
            index += (cIndex - lIndex) > 0 ? 1 : -1;
         }

         // get Index of last.
         // keep adding one and linearly interpolate
         index = (int)(m(0, 0) / COLS *
                       Pose::EXCLUSION_RESOLUTION);
         if (index >= 0 && index < Pose::EXCLUSION_RESOLUTION) {
            if (m(1, 0) < points[index]) {
               points[index] = m(1, 0);
            }
         }
         last = m;
      }
   }
}

std::pair<int, int> Kinematics::calculateHorizon(
   const boost::numeric::ublas::matrix<float> &c2w) {
   boost::numeric::ublas::matrix<float> transform = createWorldToFOVTransform(c2w);

   // set up horizon points in world coordinates.
   // the idea is to now convert these points into camera space and then
   // project them onto pixels to find out where the horizon is
   // Note: the -1000 and 1000 are completely arbitrary...
   // Assume horizon is in the top image, not bottom
   boost::numeric::ublas::matrix<float> pixel1 = vec4<float>(-1000, INT_MAX,
                                                     0, 1);
   boost::numeric::ublas::matrix<float> pixel2 = vec4<float>(1000, INT_MAX,
                                                     0, 1);

   pixel1 = fovToImageSpaceTransform(transform, pixel1, true);
   pixel2 = fovToImageSpaceTransform(transform, pixel2, true);

   // find gradient of the horizon
   boost::numeric::ublas::matrix<float> dir = pixel1 - pixel2;

   // we now convert the horizon to a nice format that vision can use.
   // it is just the two y intercepts at x = 0 and x = IMAGE_COLS
   float lambda1 = -pixel1(0, 0) / dir(0, 0);
   float lambda2 = (TOP_IMAGE_COLS - pixel1(0, 0)) / dir(0, 0);

   float y1 = lambda1 * dir(1, 0) + pixel1(1, 0);
   float y2 = lambda2 * dir(1, 0) + pixel1(1, 0);

   return std::pair<int, int>(y1, y2);
}

boost::numeric::ublas::matrix<float>
Kinematics::fovToImageSpaceTransform(
   const boost::numeric::ublas::matrix<float> &transform,
   const boost::numeric::ublas::matrix<float> &point, bool top) {

   // Constants
   const int COLS = (top) ? TOP_IMAGE_COLS : BOT_IMAGE_COLS;
   const int ROWS = (top) ? TOP_IMAGE_ROWS : BOT_IMAGE_ROWS;

   // use image space transform to find the perspective scaling factor
   boost::numeric::ublas::matrix<float> pixel =
      boost::numeric::ublas::prod(transform, point);

   // divide x and y by the perspective scaling factor
   pixel(0, 0) /= pixel(3, 0);
   pixel(1, 0) /= pixel(3, 0);
   pixel(2, 0) = pixel(3, 0);
   pixel(3, 0) = 1;

   // now we have the pixel in a space that spans from (-1, 1) in the x
   // direction and (-1, 1) in the y.

   // therefore we need to scale this up to our image size which is what
   // the code below does
   float xscale = COLS / 2;
   float yscale = ROWS / 2;
   pixel(0, 0) = (pixel(0, 0)) * xscale + xscale;
   pixel(1, 0) = (pixel(1, 0)) * xscale + yscale;
   return pixel;
}


#include <boost/python.hpp>
#include <scitbx/array_family/shared.h>
#include <scitbx/vec2.h>
#include <scitbx/vec3.h>
#include <scitbx/constants.h>

namespace spotfinder { namespace distltbx { namespace boost_python {

  typedef scitbx::vec3<double> point;
  typedef scitbx::af::shared<point> vec2_list;
  typedef scitbx::vec2<int> edge;
  typedef scitbx::af::shared<edge> edge_list;

  struct edge_relation {
    point left; //vector from beam to counterclockwise corner
    point right;//vector from beam to clockwise corner
    point L_R;  //vector pointing from R to L
    point nL_R; //unit vector from R to L
    point ortho;//orthogonal vector from beam to edge
    point cross;//provide a handedness reference

    edge_relation(const point& beam,
                  const point& left_corner, const point& right_corner):
      left(left_corner - beam),
      right(right_corner - beam){
      L_R  = left - right;
      nL_R = L_R.normalize();
      double Rcosq = -right * nL_R; // for parametric expression of orthogonal
      ortho = right + Rcosq*nL_R;
      cross = right.cross(left);
    }

    double magnitude(const point& a) const {
      return std::sqrt(a * a);
    }

    double edge_distance() const {
      return std::sqrt(ortho * ortho);
    }

    double operator()(const double& radius) const {
      if (radius <= edge_distance()){
        return angle_difference(right,left);
      } else {
        //find two intersection points of radius with edge; use right triangle
        double eta = std::sqrt(radius*radius - ortho * ortho);
        point etaL = ortho + eta*nL_R;
        point etaR = ortho - eta*nL_R;
        //determine if radius/edge intersection is beyond the corner
        point trueL, trueR;
        if (angle_difference(etaL, left) > 0.) {
          trueL = etaL;
        } else {
          trueL = left;
        }
        if (angle_difference(etaR, right) > 0) {
          trueR = right;
        } else {
          trueR = etaR;
        }
        return angle_difference(right,left) -
               angle_difference(trueR,trueL);
      }
    }

    double angle_difference(const point& a, const point& b) const {
      //first determine the unsigned angle
      double denominator = (magnitude(a)*magnitude(b));
      if (denominator==0.) {return 0.0;}
      double discriminant = (a * b)/denominator;
      if (discriminant>1.) {discriminant=1.;}
      if (discriminant<-1.) {discriminant=-1.;}
      double angle = std::acos(discriminant);
      //now refer the sign to the handedness reference
      if (   (cross * (a.cross(b)))   >= 0.) {
        return angle;
      } else {
        return -angle;
      }
    }
  };

  struct geometry_2d_base{
    double pixel_size, size1, size2, xbeam, ybeam, distance, wavelength;
    vec2_list corners;
    point beamspot;
    edge_list edge_index;
    scitbx::af::shared<edge_relation> edges;

    geometry_2d_base(
      const double& pixel_size,
      const double& size1,
      const double& size2,
      const double& xbeam,
      const double& ybeam,
      const double& distance,
      const double& wavelength):
      pixel_size(pixel_size),size1(size1),size2(size2),xbeam(xbeam),
      ybeam(ybeam),distance(distance),wavelength(wavelength),
      beamspot(point(xbeam,ybeam,0.)){

      corners.push_back(point(0.,              0.,0.));
      corners.push_back(point(0.,              size2*pixel_size,0.));
      corners.push_back(point(size1*pixel_size,size2*pixel_size,0.));
      corners.push_back(point(size1*pixel_size,0.,0.));

      edge_index.push_back(edge(0,1));
      edge_index.push_back(edge(1,2));
      edge_index.push_back(edge(2,3));
      edge_index.push_back(edge(3,0));

      for (int n=0; n<4; ++n){
        edges.push_back( edge_relation(beamspot,
                                       corners[edge_index[n][0]],
                                       corners[edge_index[n][1]]) );
      }
    }

    double operator()(const double& resolution) const {
      double rad = radius(resolution);
      double total_angle = 0.;
      for (int n=0; n<4; ++n) {
        double edge_contribution = edges[n](rad);
        total_angle+=edge_contribution;
      }
      return total_angle/(2.0*scitbx::constants::pi);
    }

    double radius(const double&resolution) const {
      double theta = std::asin(wavelength/2.0/resolution);
      return distance * std::tan(2.0*theta);
    }

    //! Quick and dirty calculation of resolution at each detector edge
    /*  Dirty calculation--assumes detector is normal to the beam, so
        there is no account for two theta swing or detector tilt.
        So this will have to be redone in near future;
        want a more general treatment that can be expanded.
     */
    scitbx::af::shared<double> corner_resolutions() const {
      // detector_origin is the origin of the coordinate system.  Xtal has negative z coordinate.
      point crystal_position = beamspot + point(0.,0.,-distance);
      scitbx::af::shared<double> result;
      point beam_vector = beamspot - crystal_position;
      for (int icor = 0; icor < corners.size(); ++icor){
        point xtal_to_corner_vector = corners[icor] - crystal_position;
        double twotheta = std::acos((beam_vector * xtal_to_corner_vector)/
                                   (beam_vector.length()*xtal_to_corner_vector.length()) );
        // n * lambda = 2 d sin theta
        result.push_back( (wavelength/(2. * std::sin(twotheta/2.))) );
      }
      return result;
    }

  };

}}} // namespace spotfinder::distltbx::boost_python

namespace spotfinder { namespace distltbx { namespace boost_python {
  using namespace boost::python;
  void wrap_geometry_2d()
  {
    class_<geometry_2d_base>("geometry_2d_base", no_init)
      .def(init<const double&, const double&, const double&,
                const double&, const double&, const double&,
                const double&>(
                ( arg("pixel_size"), arg("size1"), arg("size2"),
                  arg("xbeam"), arg("ybeam"), arg("distance"),
                  arg("wavelength")
                )))
      .def("__call__", &geometry_2d_base::operator())
      .def("corner_resolutions",&geometry_2d_base::corner_resolutions)
    ;
  }

}}} // namespace spotfinder::distltbx::boost_python


/*!
\brief MinimAD a minimum purpose AD tool
Copyright (C) Sebastian Schlenkrich, June 2012
Version 2.0

MinimAD provides an Algorithmic Differentiation framework
based on C++ Template Meta Programming. It is inspired by
the instant elimination approach proposed by J. Riehme
and A. Griewank (2009).

The key feature of MinimAD is the definition of a class
template Variable with a generic type (ValueType) for
its value component. All function and derivative
evaluations are defined in the template without
specifying the concrete type.

If the class template Variable is instantiated, for
example, with double then the compiler generates the
concrete source code. By inlining the generated code
further optimizations can be performed by the compiler.
Thus, the template based AD approach is not pure
operator overloading but also a kind of source
transformation by the compiler.

An important consequence of the template definition is
the fact that the class template can be instantiated with
any object type that provides arithmetic operations. Thus,
ValueType can be an active AD enabled type or, in
particular an instantiation of the template class Variable
itself. For example, by declaring objects of type
Variable< Variable<double> > second order derivatives can
easily be evaluated.

The approach for nested template instanstantiations can
be extended for higher order derivatives if required. A
further applications could, for example, be the use of
intervall types as underlying ValueType.

Version 2.0 improves performance by incorporating static
arrays for edges in addition to std::map<>. The first
edges are realized by static arrays. This avoids STL
containers in particular for temporary variables.

*/

#ifndef MinimAD_MinimADVariable_hpp
#define MinimAD_MinimADVariable_hpp

//#define _USE_MATH_DEFINES // for Visual Studio

#include <iostream>
#include <iomanip>

#include <map>
#include <vector>
#include <iterator>

//#include <cmath>

//#define DEBUG              // low-level io debugging
//#define EXECUTABLE           // compile example and main function
#define MS_VISUAL_STUDIO     // math functions not in global scope

#ifdef MS_VISUAL_STUDIO
#include <boost/math/special_functions/erf.hpp>
#endif

namespace MinimAD {

#ifdef DEBUG
#define MSG(msg_stream) std::cerr << msg_stream << std::endl ;
#else
#define MSG(msg_stream)
#endif

#ifdef MS_VISUAL_STUDIO
    //! Some compilers don't find intrinsic double functions
    inline double exp(double x) { return std::exp(x); }
    inline double log(double x) { return std::log(x); }
    inline double sqrt(double x) { return std::sqrt(x); }
    inline double sin(double x) { return std::sin(x); }
    inline double cos(double x) { return std::cos(x); }
    inline double tan(double x) { return std::tan(x); }
    inline double atan2(double y, double x) { return std::atan2(y,x); }
    inline double erf(double x)     { return boost::math::erf(x); }
    inline double erf_inv(double x) { return boost::math::erf_inv(x); }
#endif

    //! A generic exception that can be thrown if something goes wrong
    class MinimADException : public std::exception {
    protected:
        std::string what_;
    public:
        MinimADException(const std::string& what) { what_ = what; }
        virtual const char* what() const throw()  { return what_.c_str(); }
        ~MinimADException() throw() {}
    };

    //! Control if static/dynamic containers are used
#define STATIC_REFERENCES
#define DYNAMIC_REFERENCES
    //! We abstract the template and Variable type to ease readability
#define TEMPLATE_CLASS_VARIABLE  template <class ValueType = double, class DerivativeType = ValueType, size_t N_ARG = 3, size_t N_RES = 3>
#define TEMPLATE_FUNCT_VARIABLE  template <class ValueType, class DerivativeType, size_t N_ARG, size_t N_RES>
#define VARIABLE_TYPE            Variable<ValueType,DerivativeType,N_ARG,N_RES>

    //! Variable defines the class template for derivative evaluations
    TEMPLATE_CLASS_VARIABLE
    class Variable {
        //! ContainerType is the unique definition of the pointer maps used
        typedef std::map< VARIABLE_TYPE* ,DerivativeType* > ContainerType;
    protected:
#ifdef STATIC_REFERENCES
        VARIABLE_TYPE  *argRef_[N_ARG];
        VARIABLE_TYPE  *resRef_[N_RES];
        DerivativeType *argDrv_[N_ARG];
        DerivativeType *resDrv_[N_RES];
#endif
#ifdef DYNAMIC_REFERENCES
        ContainerType *arguments_;                // dynamic edges to preceeding/succeeding variables
        ContainerType *results_;
#endif
        // attributes
        ValueType value_;                         // function value
        static DerivativeType zeroDerivative_;    // a zero object is required as return value
        //! This is function is called by constructors
        inline void initialise() {
#ifdef STATIC_REFERENCES
            for (size_t k=0; k<N_ARG; ++k) argRef_[k] = 0;
            for (size_t k=0; k<N_RES; ++k) resRef_[k] = 0;
#endif
#ifdef DYNAMIC_REFERENCES
            arguments_ = 0;
            results_   = 0;
#endif
        }

        //! \section Modifiers of the graph representation that add and remove edges
        //! method addResult is called only by method addArgument to ensure symetric references
        inline void addResult(const VARIABLE_TYPE &result, DerivativeType *p_derivative) {
#ifdef STATIC_REFERENCES
            //! first try static container
            for (size_t k=0; k<N_RES; ++k) {
                if (!resRef_[k]) {
                    resRef_[k] = &(const_cast<VARIABLE_TYPE &>(result));
                    resDrv_[k] = p_derivative;
                    return;
                }
            }
#endif
#ifdef DYNAMIC_REFERENCES
            //! if this does not work use dynamic container
            if (!results_) results_ = new ContainerType;
            //(const_cast<VARIABLE_TYPE &>(argument)).results_[this] = p_derivative;
            results_->insert(std::pair< VARIABLE_TYPE* ,DerivativeType* >(&(const_cast<VARIABLE_TYPE &>(result)),p_derivative));
            return;
#endif
            //! In case we don't have a dynamic container and we could not insert something is wrong
            MSG( "Variable Add Result Error: " << this << " " << &result )
                throw MinimADException("ERROR! Variable result can not be added.");
        }

        //! method addArgumwnts is the interface for graph creation
        //! this is called by constructors, assignment operators, and elimitate() method
        inline void addArgument(const VARIABLE_TYPE &argument, const DerivativeType &derivative) {
#ifdef STATIC_REFERENCES
            //! find argument in static container and update derivative
            size_t idx = N_ARG;
            for (size_t k=0; k<N_ARG; ++k) {
                if (argRef_[k]==&argument) {
                    idx = k;
                    break;
                }
            }
            if (idx<N_ARG) {
                (*(argDrv_[idx])) += derivative;
                return;
            }
#endif
#ifdef DYNAMIC_REFERENCES
            //! also try the dynamic container
            if (arguments_) {
                typename ContainerType::iterator it = arguments_->find(&(const_cast<VARIABLE_TYPE &>(argument)));
                if (it!=arguments_->end()) {
                    (*it->second) += derivative;
                    return;
                }
            }
#endif
            //! now we have to add a new argument and its derivative to the graph
            DerivativeType *p_derivative = new DerivativeType(derivative);
#ifdef STATIC_REFERENCES
            for (size_t k=0; k<N_ARG; ++k) {
                if (!argRef_[k]) {
                    argRef_[k] = &(const_cast<VARIABLE_TYPE &>(argument));
                    argDrv_[k] = p_derivative;
                    (const_cast<VARIABLE_TYPE &>(argument)).addResult(*this,p_derivative);
                    return;
                }
            }
#endif
#ifdef DYNAMIC_REFERENCES
            if (!arguments_) arguments_ = new ContainerType;
            //(*arguments_)[&(const_cast<VARIABLE_TYPE &>(argument))] = p_derivative;
            arguments_->insert( std::pair< VARIABLE_TYPE* ,DerivativeType* > (&(const_cast<VARIABLE_TYPE &>(argument)), p_derivative));
            (const_cast<VARIABLE_TYPE &>(argument)).addResult(*this,p_derivative);
            return;
#endif
            //! In case we don't have a dynamic container and we could not insert something is wrong
            MSG( "Variable Add Argument Error: " << this << " " << &argument )
                throw MinimADException("ERROR! Variable argument can not be added.");
        }
        //! method removeResults encapsulates the removal process of results called by elliminate()

        inline void removeResult(const VARIABLE_TYPE &result) {
#ifdef STATIC_REFERENCES
            for (size_t k=0; k<N_RES; ++k) {
                if (resRef_[k]==&result) {
                    resRef_[k] = 0;
                    return;
                }
            }
#endif
#ifdef DYNAMIC_REFERENCES
            if (results_) {
                typename ContainerType::iterator it = results_->find(&(const_cast<VARIABLE_TYPE &>(result)));
                if (it!=results_->end()){
                    results_->erase(it);
                    return;
                }
            }
#endif
            //! \warning if there is nothing to remove sonething is wrong with the function call
            MSG( "Variable Remove result Error: " << this << " " << &result )
                throw MinimADException("ERROR! Variable Result can not be removed.");
        }

        //! method removeArguement encapsulates the removal process of arguments called by elliminate()
        inline void removeArgument(const VARIABLE_TYPE &argument) {
#ifdef STATIC_REFERENCES
            for (size_t k=0; k<N_ARG; ++k) {
                if (argRef_[k]==&argument) {
                    argRef_[k] = 0;
                    return;
                }
            }
#endif
#ifdef DYNAMIC_REFERENCES
            if (arguments_) {
                typename ContainerType::iterator it = arguments_->find(&(const_cast<VARIABLE_TYPE &>(argument)));
                if (it!=arguments_->end()) {
                    arguments_->erase(it);
                    return;
                }
            }
#endif
            //! \warning if there is nothing to remove sonething is wrong with the function call
            MSG( "Variable Remove argument Error: " << this << " " << &argument )
                throw MinimADException("ERROR! Variable Argument can not be removed.");
        }

        inline void eliminate() {
#ifdef STATIC_REFERENCES
            //! first remove current node from static graph
            for (size_t k=0; k<N_RES; ++k) if (resRef_[k]) resRef_[k]->removeArgument( *this );
            for (size_t k=0; k<N_ARG; ++k) if (argRef_[k]) argRef_[k]->removeResult( *this );
            //! in a second step conect incoming and outgoing nodes applying the chain rule
            for (size_t i=0; i<N_RES; ++i) {
                if (resRef_[i]) {
                    for (size_t j=0; j<N_ARG; ++j) {
                        if (argRef_[j]) {
                            resRef_[i]->addArgument( *argRef_[j], (*resDrv_[i]) * (*argDrv_[j]) );
                            VARIABLE_TYPE::eliminations++;
                        }
                    }
                }
            }
#endif
#ifdef DYNAMIC_REFERENCES
            //! first remove current node from dynamic graph
            if (results_) for (typename ContainerType::iterator it=results_->begin(); it!=results_->end(); ++it) it->first->removeArgument( *this );
            if (arguments_) for (typename ContainerType::iterator it=arguments_->begin(); it!=arguments_->end(); ++it) it->first->removeResult( *this );
            //! in a second step connect incoming and outgoing nodes applying the chain rule
            if (arguments_ && results_) {
                for (typename ContainerType::iterator i=results_->begin(); i!=results_->end(); ++i) {
                    for (typename ContainerType::iterator j=arguments_->begin(); j!=arguments_->end(); ++j) {
                        i->first->addArgument( *(j->first), (*i->second) * (*j->second) );
                        VARIABLE_TYPE::eliminations++;
                    }
                }
            }
#endif
#ifdef STATIC_REFERENCES
#ifdef DYNAMIC_REFERENCES
            //! connect static results with dynamic arguments
            if (arguments_) {
                for (size_t i=0; i<N_RES; ++i) {
                    if (resRef_[i]) {
                        for (typename ContainerType::iterator j=arguments_->begin(); j!=arguments_->end(); ++j) {
                            resRef_[i]->addArgument( *(j->first), (*resDrv_[i]) * (*j->second) );
                            VARIABLE_TYPE::eliminations++;
                        }
                    }
                }
            }
            //! connect dynamic results with static arguments
            if (results_) {
                for (typename ContainerType::iterator i=results_->begin(); i!=results_->end(); ++i) {
                    for (size_t j=0; j<N_ARG; ++j) {
                        if (argRef_[j]) {
                            i->first->addArgument( *argRef_[j], (*i->second) * (*argDrv_[j]) );
                            VARIABLE_TYPE::eliminations++;
                        }
                    }
                }
            }
#endif // STATIC_REFERENCES
#endif // DYNAMIC_REFERENCES
            //! we have to clean up memory manually which is allocated in addArguments() and addResults()
            //! this is the place to modify the higher order elimination sequence
#ifdef STATIC_REFERENCES
            for (size_t k=0; k<N_RES; ++k) {
                if (resRef_[k]) {
                    delete resDrv_[k];
                    resRef_[k] = 0;
                }
            }
            for (size_t k=0; k<N_ARG; ++k) {
                if (argRef_[k]) {
                    delete argDrv_[k];
                    argRef_[k] = 0;
                }
            }
#endif
#ifdef DYNAMIC_REFERENCES
            if (results_) {
                for (typename ContainerType::iterator it=results_->begin(); it!=results_->end(); ++it) delete (it->second);
                results_->clear();
                delete results_;
                results_ = 0;
            }
            if (arguments_) {
                for (typename ContainerType::iterator it=arguments_->begin(); it!=arguments_->end(); ++it) delete (it->second);
                arguments_->clear();
                delete arguments_;
                arguments_ = 0;
            }
#endif
        } // eliminate()

    public:
        static double eliminations;
        //! \section Constructors for Variable objects
        //! Internal unary constructor
        Variable( const ValueType&                            value,
                  const VARIABLE_TYPE&                        argument,
                  const DerivativeType&                       derivative )
            : value_(value) {
            MSG( "UC: " << this << " " << value << " " << &argument )
            initialise();
            this->addArgument( argument, derivative );
        }
        //! Internal binary constructor
        Variable( const ValueType&                            value,
                  const VARIABLE_TYPE&   			          argument1,
                  const VARIABLE_TYPE&                        argument2,
                  const DerivativeType&                       derivative1,
                  const DerivativeType&                       derivative2  )
            : value_(value) {
            MSG( "BC: " << this << " " << value << " " << &argument1 << " " << &argument2 )
            initialise();
            this->addArgument( argument1, derivative1 );
            this->addArgument( argument2, derivative2 );
        }
        //! Standard constructor
        Variable() : value_(0) {
            MSG( "SC: " << this )
            initialise();
        }
        //! ValueType constructor
        Variable( const ValueType &argument)
            : value_(argument) {
            MSG( "VC: " << this << " " << argument )
            initialise();
        }
        //! Copy constructor
        Variable( const VARIABLE_TYPE &argument )
            : value_(argument.value_) {
            MSG( "CC: " << this << " " << argument.value_ << " " << &argument )
            initialise();
            this->addArgument( argument, (DerivativeType)1);
        }
        //! Destructor
        ~Variable() {
            MSG( "DR: " << this )
            eliminate();
        }
        //! \section Inspectors
        //! Returning a reference to the value component
        inline const ValueType& value() const { return value_; }
        inline const ValueType& val() const { return value_; }
        //! Returning a reference to the derivative object of this w.r.t. y
        inline const DerivativeType& operator % (const VARIABLE_TYPE &y ) const {
#ifdef STATIC_REFERENCES
            //! search for edge in static container
            for (size_t k=0; k<N_ARG; ++k) {
                if (argRef_[k]==&y) {
                    return *argDrv_[k];
                }
            }
#endif
#ifdef DYNAMIC_REFERENCES
            if (arguments_) {
                typename ContainerType::iterator arg_it = (const_cast< VARIABLE_TYPE* >(this))->arguments_->find(&(const_cast< VARIABLE_TYPE& >(y)));
                if (arg_it!=this->arguments_->end()) return (*arg_it->second);
            }
#endif
            /*
            typename ContainerType::iterator arg_it = (const_cast< VARIABLE_TYPE* >(this))->arguments_->find(&(const_cast< VARIABLE_TYPE& >(y)));
            if (arg_it!=this->arguments_->end()) {
            MSG("arg: " << this << " derivative: " << (*arg_it->second) )
            }
            typename ContainerType::iterator res_it = (const_cast< VARIABLE_TYPE& >(y)).results_->find((const_cast< VARIABLE_TYPE* >(this)));
            if (res_it!=y.results_->end()) {
            MSG("res: " << &y << " derivative: " << (*res_it->second) )
            }
            if (arg_it!=this->arguments_->end()) return (*arg_it->second);
            if (res_it!=y.results_->end()) return (*res_it->second);
            */
            return zeroDerivative_;
        }
        //! \section Assignment operators
        //! Variable = Variable assignment
        inline VARIABLE_TYPE& operator = (const VARIABLE_TYPE& argument) {
            MSG( "Variable Active Assignment: " << this << " " << &argument )
            if (this != &argument) {
                this->eliminate();
                this->value_ = argument.value_;
                this->addArgument( argument, (DerivativeType)1);
            }
            // otherwise do nothing
            return *this;
        }
        //! Variable = ValueType assignment
        inline VARIABLE_TYPE& operator = (const ValueType& argument) {
            MSG( "Variable ValueType Assignment: " << this << " " << argument )
            this->eliminate();
            this->value_ = argument;
            return *this;
        }
        //! \section Arithmetic-Assignment operators
        //! \subsection Variable (OP)= Variable
        inline VARIABLE_TYPE& operator += (const VARIABLE_TYPE &x) { return *this = *this + x; }
        inline VARIABLE_TYPE& operator -= (const VARIABLE_TYPE &x) { return *this = *this - x; }
        inline VARIABLE_TYPE& operator *= (const VARIABLE_TYPE &x) { return *this = *this * x; }
        inline VARIABLE_TYPE& operator /= (const VARIABLE_TYPE &x) { return *this = *this / x; }
        //! \subsection Variable (OP)= ValueType
        inline VARIABLE_TYPE& operator += (const ValueType &x) { return *this = *this + x; }
        inline VARIABLE_TYPE& operator -= (const ValueType &x) { return *this = *this - x; }
        inline VARIABLE_TYPE& operator *= (const ValueType &x) { return *this = *this * x; }
        inline VARIABLE_TYPE& operator /= (const ValueType &x) { return *this = *this / x; }
        //! \section Relational operators
        //! \subsection Variable x Variable
        inline bool operator >  (const VARIABLE_TYPE &x) { return this->value_ >  x.value_; }
        inline bool operator >= (const VARIABLE_TYPE &x) { return this->value_ >= x.value_; }
        inline bool operator <  (const VARIABLE_TYPE &x) { return this->value_ <  x.value_; }
        inline bool operator <= (const VARIABLE_TYPE &x) { return this->value_ <= x.value_; }
        inline bool operator != (const VARIABLE_TYPE &x) { return this->value_ != x.value_; }
        inline bool operator == (const VARIABLE_TYPE &x) { return this->value_ == x.value_; }
        //! \subsection Variable x Variable (for left const operands)
        inline friend bool operator >  (const VARIABLE_TYPE &x, const VARIABLE_TYPE &y) { return x.value_ >  y.value_; }
        inline friend bool operator >= (const VARIABLE_TYPE &x, const VARIABLE_TYPE &y) { return x.value_ >= y.value_; }
        inline friend bool operator <  (const VARIABLE_TYPE &x, const VARIABLE_TYPE &y) { return x.value_ <  y.value_; }
        inline friend bool operator <= (const VARIABLE_TYPE &x, const VARIABLE_TYPE &y) { return x.value_ <= y.value_; }
        inline friend bool operator != (const VARIABLE_TYPE &x, const VARIABLE_TYPE &y) { return x.value_ != y.value_; }
        inline friend bool operator == (const VARIABLE_TYPE &x, const VARIABLE_TYPE &y) { return x.value_ == y.value_; }
        //! \subsection Variable x ValueType
        inline bool operator >  (const ValueType &x) { return this->value_ >  x; }
        inline bool operator >= (const ValueType &x) { return this->value_ >= x; }
        inline bool operator <  (const ValueType &x) { return this->value_ <  x; }
        inline bool operator <= (const ValueType &x) { return this->value_ <= x; }
        inline bool operator != (const ValueType &x) { return this->value_ != x; }
        inline bool operator == (const ValueType &x) { return this->value_ == x; }
        //! \subsection ValueType x Variable
        inline friend bool operator >  (const ValueType &x, const VARIABLE_TYPE &y) { return x >  y.value_; }
        inline friend bool operator >= (const ValueType &x, const VARIABLE_TYPE &y) { return x >= y.value_; }
        inline friend bool operator <  (const ValueType &x, const VARIABLE_TYPE &y) { return x <  y.value_; }
        inline friend bool operator <= (const ValueType &x, const VARIABLE_TYPE &y) { return x <= y.value_; }
        inline friend bool operator != (const ValueType &x, const VARIABLE_TYPE &y) { return x != y.value_; }
        inline friend bool operator == (const ValueType &x, const VARIABLE_TYPE &y) { return x == y.value_; }
        //! \section IO steaming
        //! Apply IO only to the value component
        inline friend std::ostream& operator << (std::ostream &output, const VARIABLE_TYPE &x) {
            return output << x.value_;
        }
        //! \section Binary arithmetic operators
        //! \subsection Variable x Variable
        inline VARIABLE_TYPE operator + (const VARIABLE_TYPE &y ) {
            return VARIABLE_TYPE( this->value_ + y.value_, *this, y, (DerivativeType)1, (DerivativeType)1 );
        }
        inline VARIABLE_TYPE operator - (const VARIABLE_TYPE &y ) {
            return VARIABLE_TYPE( this->value_ - y.value_, *this, y, (DerivativeType)1, (DerivativeType)(-1) );
        }
        inline VARIABLE_TYPE operator * (const VARIABLE_TYPE &y ) {
            return VARIABLE_TYPE( this->value_ * y.value_, *this, y, y.value_, this->value_ );
        }
        inline VARIABLE_TYPE operator / (const VARIABLE_TYPE &y ) {
            ValueType tmp = this->value_ / y.value_;
            return VARIABLE_TYPE( tmp, *this, y, 1/y.value_, -tmp/y.value_ );
        }
        //! \subsection Variable x Variable (for left const operands)
        inline friend VARIABLE_TYPE operator + (const VARIABLE_TYPE &x, const VARIABLE_TYPE &y ) {
            return VARIABLE_TYPE( x.value_ + y.value_, x, y, (DerivativeType)1, (DerivativeType)1 );
        }
        inline friend VARIABLE_TYPE operator - (const VARIABLE_TYPE &x, const VARIABLE_TYPE &y ) {
            return VARIABLE_TYPE( x.value_ - y.value_, x, y, (DerivativeType)1, (DerivativeType)(-1) );
        }
        inline friend VARIABLE_TYPE operator * (const VARIABLE_TYPE &x, const VARIABLE_TYPE &y ) {
            return VARIABLE_TYPE( x.value_ * y.value_, x, y, y.value_, x.value_ );
        }
        inline friend VARIABLE_TYPE operator / (const VARIABLE_TYPE &x, const VARIABLE_TYPE &y ) {
            ValueType tmp = x.value_ / y.value_;
            return VARIABLE_TYPE( tmp, x, y, 1/y.value_, -tmp/y.value_ );
        }

        //! \subsection Variable x ValueType
        inline VARIABLE_TYPE operator + (const ValueType &y ) {
            return VARIABLE_TYPE( this->value_ + y, *this, (DerivativeType)1 );
        }
        inline VARIABLE_TYPE operator - (const ValueType &y ) {
            return VARIABLE_TYPE( this->value_ - y, *this, (DerivativeType)1 );
        }
        inline VARIABLE_TYPE operator * (const ValueType &y ) {
            return VARIABLE_TYPE( this->value_ * y, *this, y );
        }
        inline VARIABLE_TYPE operator / (const ValueType &y ) {
            return VARIABLE_TYPE( this->value_ / y, *this, 1/y );
        }
        //! \subsection ValueType x Variable
        inline friend VARIABLE_TYPE operator + (const ValueType &x, const VARIABLE_TYPE &y ) {
            return VARIABLE_TYPE( x + y.value_, y, (DerivativeType)1 );
        }
        inline friend VARIABLE_TYPE operator - (const ValueType &x, const VARIABLE_TYPE &y ) {
            return VARIABLE_TYPE( x - y.value_, y, (DerivativeType)(-1) );
        }
        inline friend VARIABLE_TYPE operator * (const ValueType &x, const VARIABLE_TYPE &y ) {
            return VARIABLE_TYPE( x * y.value_, y, x );
        }
        inline friend VARIABLE_TYPE operator / (const ValueType &x, const VARIABLE_TYPE &y ) {
            ValueType tmp = x / y.value_;
            return VARIABLE_TYPE( tmp, y, -tmp/y.value_ );
        }
        //! \section Unary arithmetic operators
        inline friend VARIABLE_TYPE operator + (const VARIABLE_TYPE &x) {
            return VARIABLE_TYPE( x.value_, x, (DerivativeType)1 );
        }
        inline friend VARIABLE_TYPE operator - (const VARIABLE_TYPE &x) {
            return VARIABLE_TYPE( -x.value_, x, (DerivativeType)(-1) );
        }
        //! \section Intrinsic functions
        inline friend VARIABLE_TYPE fabs(const VARIABLE_TYPE &x) {
            return (x.value_>=0.0) ? x : (-x);
        }
        inline friend VARIABLE_TYPE exp(const VARIABLE_TYPE &x) {
            ValueType tmp = exp(x.value_);
            return VARIABLE_TYPE( tmp, x, tmp );
        }
        inline friend VARIABLE_TYPE log(const VARIABLE_TYPE &x) {
            return VARIABLE_TYPE( log(x.value_), x, 1/x.value_ );
        }
        inline friend VARIABLE_TYPE sqrt(const VARIABLE_TYPE &x) {
            ValueType tmp = sqrt(x.value_);
            return VARIABLE_TYPE( tmp, x, 0.5/tmp );
        }
        inline friend VARIABLE_TYPE sin(const VARIABLE_TYPE &x) {
            return VARIABLE_TYPE( sin(x.value_), x, cos(x.value_) );
        }
        inline friend VARIABLE_TYPE cos(const VARIABLE_TYPE &x) {
            return VARIABLE_TYPE( cos(x.value_), x, -sin(x.value_) );
        }
        inline friend VARIABLE_TYPE tan(const VARIABLE_TYPE &x) {
            ValueType tmp = tan(x.value_);
            return VARIABLE_TYPE( tmp, x, 1+tmp*tmp );
        }
        inline friend VARIABLE_TYPE erf(const VARIABLE_TYPE &x) {
            return VARIABLE_TYPE( erf(x.value_), x, M_2_SQRTPI * exp(-x.value_*x.value_) );
        }
        inline friend VARIABLE_TYPE erfc(const VARIABLE_TYPE &x) {
            return VARIABLE_TYPE( erfc(x.value_), x, (-M_2_SQRTPI) * exp(-x.value_*x.value_) );
        }
        inline friend VARIABLE_TYPE atan2(const VARIABLE_TYPE &y, const VARIABLE_TYPE &x) {
            ValueType den = x.value_ * x.value_ + y.value_ * y.value_;
            return VARIABLE_TYPE( atan2(y.value_,x.value_), y, x, x.value_/den, -y.value_/den );
        }
        //! \section TODO: further intrinsic functions...
    };   // class Variable

    //! \section definition of static class attributes
    TEMPLATE_FUNCT_VARIABLE
    DerivativeType VARIABLE_TYPE::zeroDerivative_(0);
    TEMPLATE_FUNCT_VARIABLE
    double VARIABLE_TYPE::eliminations = 0;

    //! \section double cast
    template <class Type> inline double value        ( Type   x ) { return value(x.value()); }
    template <>           inline double value<double>( double x ) { return x;                }

    //! \section Additional double x Variable operators; required for more complex Variable instantiations
    //! \subsection Variable x double
    TEMPLATE_FUNCT_VARIABLE
    inline VARIABLE_TYPE operator + (const double &x, const VARIABLE_TYPE &y ) {
        return VARIABLE_TYPE( x + y.value(), y, (DerivativeType)1 );
    }
    TEMPLATE_FUNCT_VARIABLE
    inline VARIABLE_TYPE operator - (const double &x, const VARIABLE_TYPE &y ) {
        return VARIABLE_TYPE( x - y.value(), y, (DerivativeType)(-1) );
    }
    TEMPLATE_FUNCT_VARIABLE
    inline VARIABLE_TYPE operator * (const double &x, const VARIABLE_TYPE &y ) {
        return VARIABLE_TYPE( x * y.value(), y, (DerivativeType)(x) );
    }
    TEMPLATE_FUNCT_VARIABLE
    inline VARIABLE_TYPE operator / (const double &x, const VARIABLE_TYPE &y ) {
        ValueType tmp = x / y.value();
        return VARIABLE_TYPE( tmp, y, -tmp/y.value() );
    }
    //! \subsection double x Variable
    TEMPLATE_FUNCT_VARIABLE
    inline VARIABLE_TYPE operator + (const VARIABLE_TYPE &x, const double &y ) {
        return VARIABLE_TYPE( x.value() + y, x, (DerivativeType)1 );
    }
    TEMPLATE_FUNCT_VARIABLE
    inline VARIABLE_TYPE operator - (const VARIABLE_TYPE &x, const double &y ) {
        return VARIABLE_TYPE( x.value() - y, x, (DerivativeType)1 );
    }
    TEMPLATE_FUNCT_VARIABLE
    inline VARIABLE_TYPE operator * (const VARIABLE_TYPE &x, const double &y ) {
        return VARIABLE_TYPE( x.value() * y, x, (DerivativeType)y );
    }
    TEMPLATE_FUNCT_VARIABLE
    inline VARIABLE_TYPE operator / (const VARIABLE_TYPE &x, const double &y ) {
        return VARIABLE_TYPE( x.value() / y, x, (DerivativeType)1/y );
    }

#undef TEMPLATE_CLASS_VARIABLE
#undef TEMPLATE_FUNCT_VARIABLE
#undef VARIABLE_TYPE

}   // namespace MinimAD

#endif   // MinimAD_MinimADVariable_hpp

#ifdef EXECUTABLE

// Dummy userdefined double type
template <class Type>
class MyDouble {
public:
    Type value_;
    //! Standard constructor
    MyDouble() : value_(0) { }
    //! ValueType constructor
    MyDouble( const Type &argument) : value_(argument) { }
    //! Copy constructor
    MyDouble( const MyDouble<Type> &argument ) : value_(argument.value_) { }
};

// 2D Rosenbrock function
// f(x,y)    = 100(x^2 - y)^2 + (x - 1)^2
// df/dx     = 400x(x^2 - y) + 2(x - 1)
// df/dy     = -200(x^2 - y)
// d^2f/dx^2 = 400(3x^2 - y) + 2
// d^2f/dxdy = -400x = d^2f/dydxs
// d^2f/dy^2 = 200
template <class Type>
Type rosenbrockFunction(Type& x, Type& y) {
    return (x*x - y)*(x*x - y)*100 + (x - 1.0)*(x - 1.0);
}

#include <time.h>

template <class Type>
void poissonTest(std::vector<Type>& x, std::vector<Type>& y, size_t dim) {
    if (dim<2) return;
    Type a_(-1.0), b_(2.0), c_(-1.0);
    std::vector<Type> z(dim), a(dim), b(dim), c(dim);
    x.resize(dim);
    y.resize(dim);
    // initialisation
    for (size_t i=0; i<dim; ++i) {
        x[i] = 1.0;
        a[i] = a_;
        b[i] = b_;
        c[i] = c_;
    }
    // in place LU decomposition; no error handling if LU decomposition does not exist
    for (size_t i=1; i<dim; ++i) {
        a[i] /= b[i-1];
        b[i] -= c[i-1]*a[i];
    }
    // forward substitution
    z[0] = x[0];
    for (size_t i=1; i<dim; ++i) z[i] = x[i] - a[i]*z[i-1];
    // backward substitution
    y[dim-1] = z[dim-1]/b[dim-1];
    for (int i=dim-2; i>=0; --i) y[i] = (z[i] - c[i]*y[i+1])/b[i];
    // elimination
    //for (size_t i=0; i<dim; ++i) c[i] = MinimAD::value(c[i]);
    //for (size_t i=0; i<dim; ++i) b[i] = 0.0;
    //for (size_t i=0; i<dim; ++i) a[i] = 0.0;
    //for (size_t i=0; i<dim; ++i) x[i] = 0.0;
    //for (size_t i=dim; i>0; --i) z[i-1] = 0.0;
    // multiplication
    /*
    z[0] = 2.0*y[0] - y[1];
    for (size_t i=1; i<dim-1; ++i)  z[i] = -y[i-1] + 2.0*y[i] - y[i+1];
    z[dim-1] = -y[dim-2] + 2.0*y[dim-1];
    // copy result
    for (size_t i=0; i<dim; ++i) y[i] = z[i];
    */
    return;
}

void run_poissonTest(size_t base, size_t count) {
    std::vector< MinimAD::Variable<double> > x, y;
    clock_t start, end;
    size_t dim=base;
    double res;
    std::cout << "Poisson Test" << std::endl;
    for (size_t k=0; k<count; ++k) {
        MinimAD::Variable<double>::eliminations = 0.0;
        res = 0.0;
        start = clock();
        poissonTest(x,y,dim);
        for (size_t i=0; i<dim; ++i) {
            for (size_t j=0; j<dim; ++j) {
                res += y[i] % x[j];
            }
        }
        end = clock();
        std::cout << "  dim = "      << dim
            << "  res = "      << res
            << "  elims = "    << MinimAD::Variable<double>::eliminations
            << "  CPU-sec. = " << 1.0*(end - start)/CLOCKS_PER_SEC
            << std::endl;
        dim *= base;
    }
}

template <typename Type> inline
    Type Phi( Type x) { return 0.5*(erf(x*M_SQRT1_2)+1.0); }

template <typename Type> inline
    Type PhiInv(const Type x) { return M_SQRT2*erf_inv(2.0*x-1.0); }

template <typename Type> inline
    Type normalExpectation( std::vector<Type>&  v,  //  payoff
    size_t              dim
    ) {
        std::vector<double> x(dim);  //  grid points of payoff
        double x0=0, s=3.0;
        for (size_t i=0; i<dim; ++i) x[i] = (2.0*i/(dim-1)-1.0)*s + x0;
        v.resize(dim);
        for (size_t i=0; i<dim; ++i) v[i] = 1.0;
        Type  mu(0.0), var(1.0), res;
        std::vector<Type> sums(x.size());
        sums[0] = (Type)0.0;
        Type Q1, Q2 = Phi((x[0]-mu)/sqrt(var));
        for (size_t i=0; i<x.size()-1; ++i) {
            Q1 = Q2;
            Q2 = Phi((x[i+1]-mu)/sqrt(var));
            sums[i+1] = sums[i] + v[i+1]*Q2 - v[i]*Q1 - 0.5*(Q1 + Q2)*(v[i+1] - v[i]) ;
            //std::cout << " i="    << i << "  " << "elims=" << MinimAD::Variable<double>::eliminations << std::endl;
        }
        res = sums[dim-1];
        Q2 = 0;
        Q1 = 0;
        for (size_t i=dim; i>0; --i) sums[i-1] = 0.0;
        //std::cout << " i="    << i << "  " << "elims=" << MinimAD::Variable<double>::eliminations << std::endl;
        return res;
}

void run_NormalExpectation(size_t base, size_t count) {
    std::vector< MinimAD::Variable<double> > v;
    MinimAD::Variable<double> y;
    std::vector<double> a;
    double b;
    clock_t start, end;
    size_t dim=base;
    double res;
    std::cout << "Normal Expectation Test double" << std::endl;
    for (size_t k=0; k<count; ++k) {
        start = clock();
        b = normalExpectation(a,dim);
        end = clock();
        std::cout << "  dim = "      << dim
            << "  CPU-sec. = " << 1.0*(end - start)/CLOCKS_PER_SEC
            << std::endl;
        dim *= base;
    }
    dim=base;
    std::cout << "Normal Expectation Test Variable<double>" << std::endl;
    for (size_t k=0; k<count; ++k) {
        res = 0.0;
        start = clock();
        y = normalExpectation(v,dim);
        for (size_t i=0; i<dim; ++i) res += y % v[i];
        end = clock();
        std::cout << "  dim = "      << dim
            << "  res = "      << res
            << "  elims = "    << MinimAD::Variable<double>::eliminations
            << "  CPU-sec. = " << 1.0*(end - start)/CLOCKS_PER_SEC
            << std::endl;
        dim *= base;
    }
}

template <typename Type> inline
    Type fibonacci(Type x1, Type x2, size_t n) {
        switch(n) {
        case 0 : return 0;
        case 1 : return x1;
        case 2 : return x2;
        }
        for (size_t k=3; k<=n; ++k) {
            if (k%2) x1 += x2/x1;
            else     x2 += x1/x2;
        }
        if (n%2) return x1;
        else     return x2;
}

void run_Fibonacci(size_t base, size_t count) {
    clock_t start, end;
    size_t n;
    std::cout << "Fibonacci Test double" << std::endl;
    n=base;
    for (size_t k=0; k<count; ++k) {
        double x1=1.0, x2=2.0, z;
        start = clock();
        z = fibonacci(x1,x2,n);
        end = clock();
        std::cout << "  n = "      << n
            << "  z = "      << z
            << "  CPU-sec. = " << 1.0*(end - start)/CLOCKS_PER_SEC
            << std::endl;
        n *= base;
    }
    std::cout << "Fibonacci Test  Variable<double>" << std::endl;
    n=base;
    for (size_t k=0; k<count; ++k) {
        MinimAD::Variable<double>::eliminations = 0;
        MinimAD::Variable<double> x1=1.0, x2=2.0, z;
        start = clock();
        z = fibonacci(x1,x2,n);
        end = clock();
        std::cout << "  n = "      << n
            << "  z = "      << z
            << "  elims = "    << MinimAD::Variable<double>::eliminations
            << "  CPU-sec. = " << 1.0*(end - start)/CLOCKS_PER_SEC
            << std::endl;
        n *= base;
    }

}

int main() {
    std::cout << "MinimAD (C) Sebastian Schlenktich (2011)"<< std::endl;
    std::cout << std::endl;

    // Test case 1, Rosenbrock function
    typedef MinimAD::Variable<double>  Active1;
    typedef MinimAD::Variable<Active1> Active2;
    typedef MinimAD::Variable<Active2> Active3;
    Active2 t;
    t = 1;
    Active3 x, y, z;
    std::cout << "+++ Text case 1 +++" <<std::endl;

    std::cout << "Rosenbrock function evaluation z = f(x,y) = 100(x^2 - y)^2 + (x - 1)^2 with x = "
        << (x=t) << " and y = " << (y=t) << " (global minimum)" << std::endl;
    z = rosenbrockFunction(x, y);
    std::cout << std::endl;

    std::cout << "objective function z = " << z << std::endl;
    std::cout << std::endl;

    std::cout << "gradient [ df/dx, df/dy ] = [ (z % x)  (z % y) ] = [ "
        << (z % x) << "  " << (z % y) << " ]" << std::endl;
    std::cout << std::endl;

    std::cout << "Hessian:" << std::endl;
    std::cout << "  [ d^2f/dx^2  d^2f/dydx ] = [ ((z % x) % x.value())  ((z % y) % x.value()) ] "
        << "= [ " << std::setw(4) << ((z % x) % x.value()) << "  " << std::setw(4) << ((z % y) % x.value()) << " ]" << std::endl;
    std::cout << "  [ d^2f/dxdy  d^2f/dy^2 ] = [ ((z % x) % y.value())  ((z % y) % y.value()) ] "
        << "= [ " << std::setw(4) << ((z % x) % y.value()) << "  " << std::setw(4) << ((z % y) % y.value()) << " ]" << std::endl;
    std::cout << std::endl;

    std::cout << "Third order derivative: d^3f/dx^3 =  (((z % x) % x.value()) % x.value().value())  " << (((z % x) % x.value()) % x.value().value()) << std::endl;
    std::cout << std::endl;

    std::cout << "+++ Text case 2 +++" <<std::endl;
    t = M_PI/6;

    std::cout << "Trigonometric function evaluation y = f(x) = sin(x)/cos(x) with x = " << (x=t) << " = Pi/6" << std::endl;
    y = sin(x)/cos(x);
    std::cout << "y       = " << y                                           << std::endl;
    std::cout << "f'(x)   = " << (y % x)                                     << std::endl;
    std::cout << "f''(x)  = " << ((y % x) % x.value())                       << std::endl;
    std::cout << "f'''(x) = " << (((y % x) % x.value()) % x.value().value()) << std::endl;
    std::cout << "Check for consistency y = f(x) = tan(x) with x = " << (x=t) << " = Pi/6" << std::endl;
    y = tan(x);
    std::cout << "y       = " << y << std::endl;
    std::cout << "f'(x)   = " << (y % x)                                     << std::endl;
    std::cout << "f''(x)  = " << ((y % x) % x.value())                       << std::endl;
    std::cout << "f'''(x) = " << (((y % x) % x.value()) % x.value().value()) << std::endl;
    std::cout << std::endl;

    std::cout << "+++ Text case 3 +++" <<std::endl;
    t = 1;
    std::cout << "Exponential function evaluation y = f(x) = exp(x) with x = " << (x=t) << std::endl;
    y = exp(x);
    std::cout << "y       = " << y                                           << std::endl;
    std::cout << "f'(x)   = " << (y % x)                                     << std::endl;
    std::cout << "f''(x)  = " << ((y % x) % x.value())                       << std::endl;
    std::cout << "f'''(x) = " << (((y % x) % x.value()) % x.value().value()) << std::endl;
    std::cout << std::endl;
    {
        std::cout << "+++ Text case 4 +++" <<std::endl;
        MinimAD::Variable<double> x, y, z;
        double h;
        std::cout << "g(y,x) = atan2(y,x) function with y = " << (y=1) << ", x = " << (x=1)
            << ", and h = " << (h=1.0e-8) << std::endl;
        std::cout << "double   [ g, g_y, g_x ] = [ "
            << (atan2(y.value(),x.value())) << ", "
            << ((atan2(y.value()+h,x.value())-atan2(y.value()-h,x.value()))/h/2) << ", "
            << ((atan2(y.value(),x.value()+h)-atan2(y.value(),x.value()-h))/h/2) << "] "
            << std::endl;
        z=atan2(y,x);
        std::cout << "Variable [ g, g_y, g_x ] = [ "
            << (z) << ", " << (z%y) << ", " << (z%x) << "] " << std::endl;
    }
    /*
    Active2 t1, t2, t3;
    t1 = 3; t2 = 2;
    t3 = t1 * t2;
    std::cout << "(t3 % t1) = " << (t3 % t1) << std::endl;
    std::cout << "(t3 % t2) = " << (t3 % t2) << std::endl;
    std::cout << "((t3 % t1) % t1.value()) = " << ((t3 % t1) % t1.value()) << std::endl;
    std::cout << "((t3 % t2) % t2.value()) = " << ((t3 % t2) % t2.value()) << std::endl;
    std::cout << "((t3 % t1) % t2.value()) = " << ((t3 % t1) % t2.value()) << std::endl;

    for (size_t k=0; k<10; ++k)
    std::cout << "fibonacci(1,2,"<< k <<") = " << fibonacci(1,2,k) << std::endl;

    //run_NormalExpectation(2, 16);
    run_Fibonacci(2, 20);
    */
    return 0;
}

#endif   // EXECUTABLE

/*  Example output

d90329@t4nb117 ~/my/PROJECTS/MinimAD
$ g++ -o MinimAD MinimADVariable.cpp

d90329@t4nb117 ~/my/PROJECTS/MinimAD
$ ./MinimAD
MinimAD (C) Sebastian Schlenktich (2011)

+++ Text case 1 +++
Rosenbrock function evaluation z = f(x,y) = 100(x^2 - y)^2 + (x - 1)^2 with x = 1 and y = 1 (global minimum)

objective function z = 0

gradient [ df/dx, df/dy ] = [ (z % x)  (z % y) ] = [ 0  -0 ]

Hessian:
[ d^2f/dx^2  d^2f/dydx ] = [ ((z % x) % x.value())  ((z % y) % x.value()) ] = [  802  -400 ]
[ d^2f/dxdy  d^2f/dy^2 ] = [ ((z % x) % y.value())  ((z % y) % y.value()) ] = [ -400   200 ]

Third order derivative: d^3f/dx^3 =  (((z % x) % x.value()) % x.value().value())  2400

+++ Text case 2 +++
Trigonometric function evaluation y = f(x) = sin(x)/cos(x) with x = 0.523599 = Pi/6
y       = 0.57735
f'(x)   = 1.33333
f''(x)  = 1.5396
f'''(x) = 5.33333
Check for consistency y = f(x) = tan(x) with x = 0.523599 = Pi/6
y       = 0.57735
f'(x)   = 1.33333
f''(x)  = 1.5396
f'''(x) = 5.33333

+++ Text case 3 +++
Exponential function evaluation y = f(x) = exp(x) with x = 1
y       = 2.71828
f'(x)   = 2.71828
f''(x)  = 2.71828
f'''(x) = 2.71828

*/


//---------------------------------------------------------------------------//
//!
//! \file   tstInverseMomentumConversion.cpp
//! \author Alex Robinson
//! \brief  The inverse momentum conversion unit tests
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <iostream>

// Boost Includes
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/cgs.hpp>

// FRENSIE Includes
#include "Utility_InverseMomentumUnits.hpp"
#include "Utility_RawPhysicalConstants.hpp"

using namespace Utility::Units;

//---------------------------------------------------------------------------//
// Tests
//---------------------------------------------------------------------------//
// Check that the inverse momentum units can be converted
BOOST_AUTO_TEST_CASE( convert )
{
  boost::units::quantity<Utility::Units::InverseAtomicMomentum>
    inverse_atomic_momentum_q( 1.0*inverse_mec_momentum );

  boost::units::quantity<Utility::Units::InverseMeCMomentum>
    inverse_mec_momentum_q( 1.0*inverse_atomic_momentum );

  BOOST_CHECK_CLOSE_FRACTION(
			inverse_atomic_momentum_q.value(),
			Utility::RawPhysicalConstants::fine_structure_constant,
			1e-15 );
  BOOST_CHECK_CLOSE_FRACTION(
		inverse_mec_momentum_q.value(),
		Utility::RawPhysicalConstants::inverse_fine_structure_constant,
		1e-15 );
}

//---------------------------------------------------------------------------//
// end tstInverseMomentumConversion.cpp
//---------------------------------------------------------------------------//