#include <MantaTypes.h>
#include <Model/Cameras/MatrixCamera.h>
#include <Core/Util/Args.h>
#include <Core/Exceptions/IllegalArgument.h>
#include <Interface/Context.h>
#include <Interface/MantaInterface.h>
#include <Interface/RayPacket.h>
#include <Interface/Scene.h>
#include <Core/Geometry/BBox.h>
#include <Core/Geometry/AffineTransform.h>
#include <Core/Math/MiscMath.h>
#include <Core/Math/Trig.h>
#include <Core/Util/Assert.h>
#include <Core/Util/NotFinished.h>
#include <iostream>

using namespace Manta;
using namespace std;
using SCIRun::Clamp;
float inverseProjView[4][4];
Vector target;

MatrixCamera::MatrixCamera(const Vector& eye_,
                                     const Vector& lookat_,
                                     const Vector& up_ )
  : eye( eye_ ), lookat( lookat_ ), up( up_ )
{
  haveCamera = true;
  setup();
}

MatrixCamera::MatrixCamera(const vector<string>& args)
{
  haveCamera = false;
  bool gotEye=false;
  bool gotLookat=false;
  bool gotUp=false;
  normalizeRays=false;
  int argc=static_cast<int>(args.size());
  for (size_t i=0; i<argc; i++) {
    string arg=args[i];
    if (arg=="-normalizeRays") {
      normalizeRays=true;
    } else {
      throw IllegalArgument("MatrixCamera", i, args);
    }
  }
  setup();
}

MatrixCamera::~MatrixCamera()
{
}

Camera* MatrixCamera::create(const vector<string>& args)
{
  return new MatrixCamera(args);
}

void MatrixCamera::preprocess(const PreprocessContext& context)
{
    haveCamera = true;
}

void MatrixCamera::getBasicCameraData(BasicCameraData& cam) const
{

}

void MatrixCamera::setBasicCameraData(BasicCameraData cam)
{

}

void MatrixCamera::reset( const Vector &eye_, const Vector &up_,
                               const Vector &lookat_ )
{

}

void MatrixCamera::setMatrix(float incomingMatrix[16])
{
  int cnt=0;
  for(int i=0;i<16;i++)
  {
    if(i!=0 && i%4==0)
    {
      cnt++;
    } 
    inverseProjView[cnt][i%4]=incomingMatrix[i];
  }
}

void MatrixCamera::setup()
{

/* Working Matrix data
  inverseProjView[0][0]=  -0.408248  ;
  inverseProjView[0][1]= -0.151846 ;
  inverseProjView[0][2]= -14.985 ;
  inverseProjView[0][3]= 14.3587 ; 

  inverseProjView[1][0]= 0.408248 ; 
  inverseProjView[1][1]= -0.151846 ;
  inverseProjView[1][2]= -14.985 ;
  inverseProjView[1][3]=  14.3587 ;

  inverseProjView[2][0]= -2.98023e-08  ;
  inverseProjView[2][1]= 0.535928  ;
  inverseProjView[2][2]= -9.99002 ;
  inverseProjView[2][3]= 9.63807 ;

  inverseProjView[3][0]= -1.49012e-08 ;
  inverseProjView[3][1]= -6.33299e-08 ;
  inverseProjView[3][2]=  -4.99501  ;
  inverseProjView[3][3]= 5.00501 ;
*/
}

void MatrixCamera::makeRays(RayPacket& rays) const
{
  //cout<<"Entering: makeRays"<<endl;
  ASSERT(rays.getFlag(RayPacket::HaveImageCoordinates));
//  ASSERT(rays.getFlag(RayPacket::ConstantEye));
//  rays.setFlag(RayPacket::ConstantOrigin);

  float temp[4];
  temp[0] = inverseProjView[0][3];
  temp[1] = inverseProjView[1][3];
  temp[2] = inverseProjView[2][3];
  temp[3] = inverseProjView[3][3];

  Vector tempEye;
  tempEye = Vector((float)temp[0]/(float)temp[3],(float)temp[1]/(float)temp[3],(float)temp[2]/(float)temp[3]);
  //tempEye.normalize(); 
 

  for(int i = rays.begin(); i < rays.end(); i++){
    temp[0] = inverseProjView[0][0]*rays.getImageCoordinates(i, 0) + inverseProjView[0][1]*rays.getImageCoordinates(i,1 ) 
              + inverseProjView[0][2] + inverseProjView[0][3];
    temp[1] = inverseProjView[1][0]*rays.getImageCoordinates(i, 0) + inverseProjView[1][1]*rays.getImageCoordinates(i,1 )
              + inverseProjView[1][2] + inverseProjView[1][3];
    temp[2] = inverseProjView[2][0]*rays.getImageCoordinates(i, 0) + inverseProjView[2][1]*rays.getImageCoordinates(i,1 )
              + inverseProjView[2][2] + inverseProjView[2][3];
    temp[3] = inverseProjView[3][0]*rays.getImageCoordinates(i, 0) + inverseProjView[3][1]*rays.getImageCoordinates(i,1 )
              + inverseProjView[3][2] + inverseProjView[3][3]; 
  

    target = Vector((float)temp[0]/(float)temp[3],(float)temp[1]/(float)temp[3],(float)temp[2]/(float)temp[3]);
    target.normalize(); 
    rays.setRay(i, tempEye, target);
  }
 rays.setFlag(RayPacket::NormalizedDirections);
 

}

void MatrixCamera::scaleFOV(Real /*scale*/)
{
  // This functionality doesn't make much sense with the matrix camera 
}

void MatrixCamera::translate(Vector t)
{
  // This functionality doesn't make much sense with the matrix camera 
}

void MatrixCamera::dolly(Real scale)
{
  // This functionality doesn't make much sense with the matrix camera 
}

void MatrixCamera::transform(AffineTransform t, TransformCenter center)
{
  // This functionality doesn't make much sense with the matrix camera 
}

void MatrixCamera::autoview(const BBox /*bbox*/)
{
  // This functionality doesn't make much sense with the matrix camera 
}

void MatrixCamera::output( std::ostream &os ) {

}