/* returns true if a point is in shadow from a light */
Color3 RayTracer::visiblePercentage(Ray ray, const shared_ptr<Light>& light, float distance) const{
    //Only computes if we are actually casting shadows
    if (m_settings.enableShadows && light->castsShadows()){
        //Increment the amount of shadow rays
        ++m_stats->shadowRays;
        //If partial coverage is enabled, we need to compute the actual percentage, otherwise just need to test visible
        if (m_settings.enablePartialCoverage){
            Color3 visibility(1,1,1);
            //the current position of shadow ray
            Point3 oldPosition = ray.origin();
            //Iterate through the surfels in between and multiply visibility by their trasmissiveness
            //Until we reach the original surface (distance <=0) or the visibility becomes 0
            while ((distance > 0) && !(visibility.clamp(0,1).isZero())){
                shared_ptr<UniversalSurfel> surfel = dynamic_pointer_cast<UniversalSurfel>(castRay(ray.bumpedRay(0.0001), distance, 0));
                //If no more surfel left, simply return
                if (!surfel){
                    return visibility;
                } else{
                    visibility *= surfel->transmissionCoefficient;
                }

                distance = distance - (surfel->position - oldPosition).magnitude();
                oldPosition = surfel->position;
                ray = Ray(oldPosition, ray.direction());
            }

            return visibility;
        } else{
            //If not doing partial coverage, just need to test visibility. Set anysurfel to 1 for faster intersection
            if(castRay(ray.bumpedRay(0.0001), distance, 1)) {
                return Color3(0,0,0);
            } else{
                return Color3(1,1,1);
            }
        }
    } else{
        //Non shadow casting always gives total visibility
        return Color3(1,1,1);
    }
}

void ScreenCalibrator::PointQueryTool::buttonCallback(int,Vrui::InputDevice::ButtonCallbackData* cbData)
	{
	if(cbData->newButtonState)
		{
		/* Get pointer to input device that caused the event: */
		Vrui::InputDevice* device=getButtonDevice(0);
		
		size_t pickResult;
		Vrui::NavTrackerState transform=Vrui::getDeviceTransformation(device);
		if(device->isRayDevice())
			pickResult=application->pickPoint(Ray(transform.getOrigin(),transform.transform(device->getDeviceRayDirection())));
		else
			pickResult=application->pickPoint(transform.getOrigin());
		
		if(pickResult!=~PickResult(0))
			{
			/* Find what type of point this is: */
			if(pickResult<application->trackingPoints.size())
				std::cout<<"Tracking point "<<pickResult<<": "<<application->trackingPoints[pickResult]<<std::endl;
			else
				{
				pickResult-=application->trackingPoints.size();
				if(pickResult<application->floorPoints.size())
					std::cout<<"Floor point "<<pickResult<<": "<<application->floorPoints[pickResult]<<std::endl;
				else
					{
					pickResult-=application->floorPoints.size();
					if(pickResult<application->screenPoints.size())
						std::cout<<"Screen point "<<pickResult<<": "<<application->screenPoints[pickResult]<<std::endl;
					else
						{
						pickResult-=application->screenPoints.size();
						if(pickResult<application->ballPoints.size())
							std::cout<<"Ball point "<<pickResult<<": "<<application->ballPoints[pickResult]<<std::endl;
						}
					}
				}
			}
		}
	}

Color Refractive::shade(ShadeRec& sr) {
	Color col = Phong::shade(sr);

	// get the texture
	if (refraction_ptr) {
		eta = 2.0f * refraction_ptr->get_color(sr).r;
	}

	Vector3D v = -sr.ray.d.normalized();
	Vector3D n = sr.nh;

	float cosTheta1 = sr.nh * v;
	float theeta = eta;
	if(cosTheta1 < 0)
	{
		n = -n;
		cosTheta1 = -cosTheta1;
		theeta = 1.0f / theeta;
	}

	float a = -1.0f / theeta;
	float term = ((cosTheta1 * cosTheta1 -1) / (theeta * theeta)) + 1;
	
	Vector3D transmitDir;
	if (term > 0.0) {
		float b = (cosTheta1 / theeta) - sqrt(term);
		transmitDir = a * v + b * n;
		transmitDir.normalize();
	}
	else {
		transmitDir = -v + 2.0f * cosTheta1 * n;
		transmitDir.normalize();
	}

	Ray transmittedRay = Ray(sr.ph+0.01f*transmitDir, transmitDir);

	col += sr.world.trace_ray(transmittedRay, sr.depth + 1);

	return col;
}

void Scene::trace(Image &img, real x1, real y1, real x2, real y2, int lev) {

    Vector zAxis = cam.getDirection().normal()*cam.getImagePlaneDistance();
    Vector yAxis = cam.getUp().normal();
    Vector xAxis = zAxis.cross(yAxis).normal();
  
    real xwidth = (x2-x1)/(img.getWidth()-1);
    real ywidth = (y2-y1)/(img.getHeight()-1);
  
    for (int i = 0;i<img.getWidth(); ++i) {
	Vector xVect = xAxis*(i*xwidth+x1);
	for (int j = 0;j<img.getHeight(); ++j) {
	    Ray tempRay = Ray(
		cam.getLocation(),
		Vector(
		    xVect
		    + yAxis*((img.getHeight()-j)*ywidth+y1)
		    + zAxis).normal());
	    img(i,j) = shade(rtrace(tempRay), tempRay, lev);
	}
    }
}

// sample ray from light
Spectrum SpotLight::sample_l( const Intersection& intersect , const LightSample* ls , Vector& dirToLight , float* distance , float* pdfw , float* emissionPdf , float* cosAtLight , Visibility& visibility ) const
{
    // direction to light
	const Vector _dirToLight = light_pos - intersect.intersect;
    
    // Normalize vec
    const float sqrLen = _dirToLight.SquaredLength();
    const float len = sqrt(sqrLen);
    dirToLight = _dirToLight / len;
    
    // direction pdf from 'intersect' to light source w.r.t solid angle
	if( pdfw )
        *pdfw = sqrLen;
    
    // emission pdf from light source w.r.t solid angle
    if( emissionPdf )
        *emissionPdf = UniformConePdf( cos_total_range );
    
    if( cosAtLight )
        *cosAtLight = 1.0f;
    
    if( distance )
        *distance = len;
    
    // update visility
    const float delta = 0.01f;
    visibility.ray = Ray( light_pos , -dirToLight , 0 , delta , len - delta );
    
	const float falloff = SatDot( dirToLight , -light_dir );
	if( falloff <= cos_total_range )
		return 0.0f;
	if( falloff >= cos_falloff_start )
		return intensity ;
	const float d = ( falloff - cos_total_range ) / ( cos_falloff_start - cos_total_range );
	if( d == 0.0f )
		return 0.0f;

	return intensity * d * d;
}

void SceneLoader::ImportLight(Scene& scene, const aiLight* const light) {
    Light L;
    aiColor3D ai_color_ka = light->mColorAmbient;
    aiColor3D ai_color_kd = light->mColorDiffuse;
    aiColor3D ai_color_ks = light->mColorSpecular;
    aiVector3D ai_direction = light->mDirection;
    aiVector3D ai_position = light->mPosition;
    aiString ai_name = light->mName;
    aiLightSourceType ai_light_type = light->mType;
    L.ka = glm::vec3(ai_color_ka[0], ai_color_ka[1], ai_color_ka[2]);
    L.ks = glm::vec3(ai_color_ks[0], ai_color_ks[1], ai_color_ks[2]);
    L.kd = glm::vec3(ai_color_kd[0], ai_color_kd[1], ai_color_kd[2]);
    L.name = std::string(ai_name.C_Str());
    L.type = static_cast<Light::LightType>(ai_light_type);
    L.ray = Ray(glm::vec3(ai_position[0], ai_position[1], ai_position[2]),
                glm::vec3(ai_direction[0], ai_direction[1], ai_direction[2]));
    L.attenuation_coefficients = glm::vec3(light->mAttenuationConstant,
                                           light->mAttenuationLinear, light->mAttenuationQuadratic);
    L.spot_coefficients = glm::vec3(light->mAngleInnerCone,
                                    light->mAngleOuterCone, 0.0f);
    scene.AddLight(L);
}

  Spectrum DiffuseLight::sample_ray_radiance(const Scene&, 
      Ray& out_ray, Normal& out_nn, float& out_pdf,
      LightRaySample sample) const
  {
    Normal shape_n;
    float ray_epsilon;
    Point shape_p = this->shape->sample_point(sample.uv_pos.x, sample.uv_pos.y,
        shape_n, ray_epsilon);
    Vector shape_wo = Vector(uniform_hemisphere_sample(sample.uv_dir.x, sample.uv_dir.y));
    if(dot(shape_wo, shape_n) < 0.f) {
      shape_wo = -shape_wo;
    }

    Normal world_n = this->shape_to_world.apply(shape_n);
    Point world_p = this->shape_to_world.apply(shape_p);
    Vector world_wo = this->shape_to_world.apply(shape_wo);

    out_ray = Ray(world_p, world_wo, ray_epsilon);
    out_nn = normalize(world_n);
    out_pdf = this->shape->point_pdf(shape_p) * INV_TWO_PI;
    return this->radiance;
  }

PositionedBlock *Terrain::CheckAim(Player *player) {
    PositionedBlock *block;
    Ray ray = Ray(player);
    
    PositionedBlock *target = NULL;
    float dist, best = 5.f;
    
    for (std::list<PositionedBlock*>::iterator it = VisibleBlocks.begin(); it != VisibleBlocks.end(); ++it) {
        block = *it; // Blocks[i];
        
        dist = ray.CheckCollision(block);
        if (0.f < dist && dist < best) {
            best = dist;
            target = block;
        }
    }
    
    if (target != NULL)
        target->marked = true;
    
    return target;
}

void Leafcutter::EvaluateRow( Ray &r, uint32_t line )
{
    uint32_t idx = 0;
    Ray ray = r;
    Vec3f throughput = Vec3f::One();
    Intersection isect;
    uint32_t depth = 0;
    while(true)
    {
        if(!_engine->Intersect(ray, &isect)) 
            return;

        BxdfUnion msu;
        isect.m->SampleReflectance(isect.dp, msu);
        Bxdf* ms = &msu;

        Vec3f wo = -ray.D;
        DifferentialGeometry &dp = isect.dp;

        if(!ms->HasDelta())
        {
            EvaluateTree(_lightTree->GetOrientedRoot(), dp, wo, isect.m, idx, line);
            EvaluateTree(_lightTree->GetDirectionalRoot(), dp, wo, isect.m, idx, line);
            return;
        }
        else
        {
            if (depth > 2)
                return;

            BxdfSample bxdfSample = ms->SampleCos(DELTA_BXDF, wo, dp, _sampler->Next2D(), _sampler->Next1D());
            if (!bxdfSample.Valid())
                return;
            throughput *= bxdfSample.brdfCos;
            ray = Ray(dp.P, bxdfSample.wi, ray.time);
            depth++;
        }
    }
}

bool computeImage()
{
	static unsigned int iPart = 0;

	if(iPart >= 64)
		return false;
    for(int j = iPart; j < screenHeight; j+=64)
	{
        for(int i = 0; i < screenWidth; i++)
		{
			float3 pixelColor = float3(0, 0, 0);
			float2 ndcPixelCentre( (2.0 * i - screenWidth) / screenWidth, (2.0 * j - screenHeight) / screenHeight );

			Camera& camera = scene.getCamera();
			Ray ray = Ray(camera.getEye(), camera.rayDirFromNdc(ndcPixelCentre));
			
			image[j*screenWidth + i] = scene.trace(ray, 0);
		}
	}
	iPart++;
	return true;
}

Color Light::Sample(Point const& point, Ray *out_ray, int row, int col) const {
  Point sample_point(0.0, 0.0, 0.0);
  double cell_width = 1.0 / cols;
  double cell_height = 1.0 / rows;

  if (area) {
    double offx = rand_double(-0.25, 0.25) / cols;
    double offy = rand_double(-0.25, 0.25) / rows;
    double x = (col * cell_width + offx) - 0.5;
    double y = (row * cell_height + offy) - 0.5;
    sample_point = Point(x, 0.0, y);
  }

  sample_point = transformation.Apply(sample_point);
  if (out_ray) {
    *out_ray = Ray(point, Normalized(sample_point - point));
    (*out_ray).max_dist = Norm(sample_point - point);
  }

  // In future, this may be deferred to a subclass for e.g. multicolored lights
  return color;
}

Ray Viewport::GetScreenRay(int x, int y) const
{
    if (!camera_)
        return Ray();

    float screenX;
    float screenY;

    if (rect_ == IntRect::ZERO)
    {
        Graphics* graphics = GetSubsystem<Graphics>();
        screenX = (float)x / (float)graphics->GetWidth();
        screenY = (float)y / (float)graphics->GetHeight();
    }
    else
    {
        screenX = float(x - rect_.left_) / (float)rect_.Width();
        screenY = float(y - rect_.top_) / (float)rect_.Height();
    }

    return camera_->GetScreenRay(screenX, screenY);
}

Spectrum GridDensityMedium::T(const Ray &_ray, Sampler &sampler) const {
    // Transform the ray into local coordinates and determine overlap interval
    // [_tMin, tMax_]
    const Bounds3f dataBounds(Point3f(0.f, 0.f, 0.f), Point3f(1.f, 1.f, 1.f));
    Ray ray = WorldToMedium(
        Ray(_ray.o, Normalize(_ray.d), _ray.tMax * _ray.d.Length()));
    Float tMin, tMax;
    if (!dataBounds.IntersectP(ray, &tMin, &tMax)) return Spectrum(1.f);
    tMin = std::max(tMin, (Float)0.f);
    tMax = std::min(tMax, ray.tMax);
    if (tMin >= tMax) return Spectrum(1.f);
    Float tr = 1.f;
    // Perform ratio tracking to estimate the transmittance value
    Float t = tMin;
    while (true) {
        t -= std::log(1 - sampler.Get1D()) * invMaxDensity;
        if (t >= tMax) break;
        Float density = Density(ray(t));
        tr *= 1.f - std::max((Float)0, sigma_t * density * invMaxDensity);
    }
    return Spectrum(tr);
}

void
PhotonMapper::render(Scene &scene)
{
    int xRes = scene.camera.xRes();
    int yRes = scene.camera.yRes();
    setRes(xRes, yRes);
    
	//clear m_rgbaBuffer
    m_rgbaBuffer.reset(Math::Color4f(1.0,1,1,1.0));
	//setup progress reporting using Platform::Progress
    
	//for each pixel generate a camera ray
    
    if (scene.photonMap == NULL && scene.specularPhotonMap == NULL) {
        scene.emit_scatterPhotons();
    }
    Platform::Progress progress = Platform::Progress("Raytracing Image", xRes*yRes);    
    for (int i=0; i < xRes; i++) {

        #pragma omp parallel for
        for (int j=0; j<yRes; j++) {
            Ray r = Ray();
            scene.camera.generateRay(r, i, j);
            Math::Vec3f col = recursiveRender(r, *(scene.photonMap), *(scene.specularPhotonMap), scene, true);
            m_rgbaBuffer(i, j) = Math::Vec4f(col.x, col.y, col.z, 1.0);
        }
        progress.step(yRes);
    }
	
	//Copy the final rendering to the texture
    glBindTexture(GL_TEXTURE_2D, m_fbo.colorTextureID(0));
    glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, m_fbo.width(), m_fbo.height(), GL_RGBA, GL_FLOAT, &m_rgbaBuffer(0,0));
    glBindTexture(GL_TEXTURE_2D, 0);    //Render to Screen
	m_fbo.blitFramebuffer(FBO_COLOR0);
//    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    
//    m_fbo.displayAlphaAsFullScreenTexture(FBO_COLOR0);

}

void RealisticCamera::RenderExitPupil(Float sx, Float sy,
                                      const char *filename) const {
    Point3f pFilm(sx, sy, 0);

    const int nSamples = 2048;
    Float *image = new Float[3 * nSamples * nSamples];
    Float *imagep = image;

    for (int y = 0; y < nSamples; ++y) {
        Float fy = (Float)y / (Float)(nSamples - 1);
        Float ly = Lerp(fy, -RearElementRadius(), RearElementRadius());
        for (int x = 0; x < nSamples; ++x) {
            Float fx = (Float)x / (Float)(nSamples - 1);
            Float lx = Lerp(fx, -RearElementRadius(), RearElementRadius());

            Point3f pRear(lx, ly, LensRearZ());

            if (lx * lx + ly * ly > RearElementRadius() * RearElementRadius()) {
                *imagep++ = 1;
                *imagep++ = 1;
                *imagep++ = 1;
            } else if (TraceLensesFromFilm(Ray(pFilm, pRear - pFilm),
                                           nullptr)) {
                *imagep++ = 0.5f;
                *imagep++ = 0.5f;
                *imagep++ = 0.5f;
            } else {
                *imagep++ = 0.f;
                *imagep++ = 0.f;
                *imagep++ = 0.f;
            }
        }
    }

    WriteImage(filename, image,
               Bounds2i(Point2i(0, 0), Point2i(nSamples, nSamples)),
               Point2i(nSamples, nSamples));
    delete[] image;
}

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

//  Sphere* S = new Sphere(16.5, Vec(0,0,0), Vec(), Vec(1,1,1)*.999, SPEC);
//  S->intersect(Ray(Vec(50,50,100),Vec(0,0,-1).norm()));
//return 0;

//  int w=1024, h=768, samps = argc==2 ? atoi(argv[1])/4 : 1; // # samples
//  int w=512, h=384, samps = argc==2 ? atoi(argv[1])/4 : 1; // # samples
  int w=256, h=192, samps = argc==2 ? atoi(argv[1])/4 : 1; // # samples

  Ray cam(Vec(50,52,295.6), Vec(0,-0.042612,-1).norm()); // cam pos, dir
  Vec cx=Vec(w*.5135/h), cy=(cx%cam.d).norm()*.5135, r;
  Vec *frame=new Vec[w*h];

#pragma omp parallel for schedule(dynamic, 1) private(r)
  for (unsigned short y=0; y<h; y++) { // Loop over image rows
    fprintf(stderr, "\rRendering (%d spp) %5.2f%%", samps*4, 100.*y/(h-1));
    for (unsigned short x=0, Xi[3]={0,0,(unsigned short)(y*y*y)}; x<w; x++) { // Loop cols
      for (int sy=0, i=(h-y-1)*w+x; sy<2; sy++) { // 2x2 subpixel rows
        for (int sx=0; sx<2; sx++, r=Vec()) {     // 2x2 subpixel cols
          for (int s=0; s<samps; s++) {
            float r1=2*erand48(Xi), dx=r1<1 ? sqrt(r1)-1 : 1-sqrt(2-r1);
            float r2=2*erand48(Xi), dy=r2<1 ? sqrt(r2)-1 : 1-sqrt(2-r2);
            Vec d = cx*(((sx+.5+dx)/2+x)/w-.5)+cy*(((sy+.5+dy)/2+y)/h-.5)+cam.d;
            r = r + radiance(Ray(cam.o+d*140, d.norm()), 0, Xi)*(1./samps);
          } // Camera rays are pushed ^^^^^ forward to start in interior
          frame[i] = frame[i] + Vec(clamp(r.x), clamp(r.y), clamp(r.z))*.25;
        }
      }
    }
  }

  // Write image to PPM file.
  FILE *f = fopen("image.ppm", "wb");
  fprintf(f, "P6\n%d %d\n%d\n", w, h, 255);
  for (int i=0; i<w*h; i++)
    fprintf(f, "%c%c%c", toInt(frame[i].x), toInt(frame[i].y), toInt(frame[i].z));
}

Bounds2f RealisticCamera::BoundExitPupil(Float pFilmX0, Float pFilmX1) const {
    Bounds2f pupilBounds;
    // Sample a collection of points on the rear lens to find exit pupil
    const int nSamples = 1024 * 1024;
    int nExitingRays = 0;

    // Compute bounding box of projection of rear element on sampling plane
    Float rearRadius = RearElementRadius();
    Bounds2f projRearBounds(Point2f(-1.5f * rearRadius, -1.5f * rearRadius),
                            Point2f(1.5f * rearRadius, 1.5f * rearRadius));
    for (int i = 0; i < nSamples; ++i) {
        // Find location of sample points on $x$ segment and rear lens element
        Point3f pFilm(Lerp((i + 0.5f) / nSamples, pFilmX0, pFilmX1), 0, 0);
        Float u[2] = {RadicalInverse(0, i), RadicalInverse(1, i)};
        Point3f pRear(Lerp(u[0], projRearBounds.pMin.x, projRearBounds.pMax.x),
                      Lerp(u[1], projRearBounds.pMin.y, projRearBounds.pMax.y),
                      LensRearZ());

        // Expand pupil bounds if ray makes it through the lens system
        if (Inside(Point2f(pRear.x, pRear.y), pupilBounds) ||
                TraceLensesFromFilm(Ray(pFilm, pRear - pFilm), nullptr)) {
            pupilBounds = Union(pupilBounds, Point2f(pRear.x, pRear.y));
            ++nExitingRays;
        }
    }

    // Return entire element bounds if no rays made it through the lens system
    if (nExitingRays == 0) {
        Info("Unable to find exit pupil in x = [%f,%f] on film.", pFilmX0,
             pFilmX1);
        return projRearBounds;
    }

    // Expand bounds to account for sample spacing
    pupilBounds = Expand(pupilBounds, 2 * projRearBounds.Diagonal().Length() /
                         std::sqrt(nSamples));
    return pupilBounds;
}

Ray Ray::getRandomRay_Sphere(Vec3f intersection, Vec3f * triangle, int depth, pair <int, int> exceptionTriangle)
{
    Vec3f en = getNormalwithRayComes(triangle, this->direction);

    //find a unit vector in the plane
    Vec3f ex(triangle[0] - intersection);
    ex.normalize();

    //another unit vector in the plane to forme a local coordiante
    Vec3f ey = cross(en, ex);
    ey.normalize();
    
    float angleN = getRandomFloat(0.0, acos(-1.0) / 2);
    float angleXY = getRandomFloat(0.0, 2 * acos(-1.0));
    
    Vec3f dirOut((ex * cos(angleXY) * sin(angleN)) +
                 (ey * sin(angleXY) * sin(angleN)) +
                 (en * cos(angleN)));
    
    dirOut.normalize();
    
    return Ray(intersection, dirOut, bshRoot, depth, exceptionTriangle, this->DBG);
}

RenderingManager::RenderingManager (OEngine::Render::OgreRenderer& _rend, const boost::filesystem::path& resDir, OEngine::Physic::PhysicEngine* engine, MWWorld::Environment& environment)
:mRendering(_rend), mObjects(mRendering), mActors(mRendering, environment), mAmbientMode(0), mDebugging(engine)
{
    mRendering.createScene("PlayerCam", 55, 5);

    // Set default mipmap level (NB some APIs ignore this)
    TextureManager::getSingleton().setDefaultNumMipmaps(5);

    // Load resources
    ResourceGroupManager::getSingleton().initialiseAllResourceGroups();

    // Turn the entire scene (represented by the 'root' node) -90
    // degrees around the x axis. This makes Z go upwards, and Y go into
    // the screen (when x is to the right.) This is the orientation that
    // Morrowind uses, and it automagically makes everything work as it
    // should.
    SceneNode *rt = mRendering.getScene()->getRootSceneNode();
    mMwRoot = rt->createChildSceneNode();
    mMwRoot->pitch(Degree(-90));
    mObjects.setMwRoot(mMwRoot);
    mActors.setMwRoot(mMwRoot);
        
    //used to obtain ingame information of ogre objects (which are faced or selected)
    mRaySceneQuery = mRendering.getScene()->createRayQuery(Ray());

    Ogre::SceneNode *playerNode = mMwRoot->createChildSceneNode ("player");
    playerNode->pitch(Degree(90));
    Ogre::SceneNode *cameraYawNode = playerNode->createChildSceneNode();
    Ogre::SceneNode *cameraPitchNode = cameraYawNode->createChildSceneNode();
    cameraPitchNode->attachObject(mRendering.getCamera());
    
    //mSkyManager = 0;
    mSkyManager = new SkyManager(mMwRoot, mRendering.getCamera());

    mPlayer = new MWRender::Player (mRendering.getCamera(), playerNode);
    mSun = 0;
}

/**
 * Implement Algorithm to test, weather there are objects between a point
 * and our light sources
 */
bool RayTracer::isHidden(LightSource* lightSource, Vector3& point) {
    //Create the ray between intersection point and light source
    Ray ray = Ray(point, (lightSource->_position - point).normalize());
    Vector3 point_moved = point + 0.01 * ray[1];
    ray[0] = point_moved;
    //Get the first intersection with any shape
    Shape* closestShape = _scene._shapes[0];
    double closestIP;
    bool hasIntersection = false;

    for (unsigned i = 0; i < _scene._shapes.size(); i++) {
        std::vector<double> intersections = _scene._shapes[i]->ensIntersect(ray);

        if (!hasIntersection && !intersections.empty()) {
            closestShape = _scene._shapes[i];
            closestIP = intersections[0];
            hasIntersection = true;
        } else if (hasIntersection && !intersections.empty()
                   && intersections[0] < closestIP) {
            closestShape = _scene._shapes[i];
            closestIP = intersections[0];
            hasIntersection = true;
        }
        if (hasIntersection) {
            Vector3 intersection = ray.getPoint(closestIP);
            //Create a vector between the two object intersections
            Vector3 point_intersection = intersection - point_moved;
            //Create a vector between object intersection and light source
            Vector3 point_light = lightSource->_position - point_moved;
            //If |point_light| > |point_intersection| there is shadow
            if (point_light.norm() > point_intersection.norm())
                return true;
        }
    }

    return false;
}