#ifndef _MYAUTOFOCUS_H
#define _MYAUTOFOCUS_H

#include <FCam/Tegra/AutoFocus.h>
#include <FCam/Tegra/Lens.h>
#include <FCam/Base.h>
#include <FCam/Image.h>
#include <android/log.h>

#define	MYAUTOFOCUS_READY 0
#define MYAUTOFOCUS_SWEEP 1
#define MYAUTOFOCUS_DONE  2

// These offsets are needed for accessing U and V parts of the pixel
#define PREVIEW_IMAGE_WIDTH  640
#define PREVIEW_IMAGE_HEIGHT 480

#define PI_U_OFFSET   (PREVIEW_IMAGE_WIDTH*PREVIEW_IMAGE_HEIGHT)
#define PI_V_OFFSET   (PREVIEW_IMAGE_WIDTH*PREVIEW_IMAGE_HEIGHT+(PREVIEW_IMAGE_WIDTH*PREVIEW_IMAGE_HEIGHT>>2))

class MyAutoFocus : public FCam::Tegra::AutoFocus {
public:
	   static const int MaxSteps = 50;

       MyAutoFocus(FCam::Tegra::Lens *l, FCam::Rect r = FCam::Rect()) : FCam::Tegra::AutoFocus(l,r) {
    	   lens = l;
    	   rect = r;

    	   state = MYAUTOFOCUS_READY;
    	   numSteps = 0;
    	   stepDir = 1; // Positive 'stepDir' moves the focus to nearer the camera

    	   nearFocus = lens->nearFocus(); // Measured in diopters = 1/(focal length)
    	   farFocus  = lens->farFocus();
    	   df = (nearFocus-farFocus)/MaxSteps; // Sweeps from far to near focus
       }

       void setROI(FCam::Rect r)
       {
    	   rect.x = r.x;
    	   rect.y = r.y;
    	   rect.width  = r.width;
    	   rect.height = r.height;
       }

       FCam::Rect getROI() { return rect; }

       void startSweep() {
    	   if (!lens) return;
    	   /* [CS478]
    	    * This method should initiate your autofocus algorithm.
    	    * Before you do that, do basic checks, e.g. is the autofocus
    	    * already engaged?
    	    */
    	   //LOG("THK: MyAutoFocus SWEEPING!\n");

    	   switch(state)
    	   {
    	   case MYAUTOFOCUS_READY:
    	   {
    		   // We turn on MyAutoFocus and reset internal counters
    		   state = MYAUTOFOCUS_SWEEP;
    		   numSteps = 0;
    		   bestScore = 0;
    		   // Set sweep starting point and direction
    		   const float midFocus = (farFocus+nearFocus)/2;
    		   float currentFocus = lens->getFocus();
    		   if(currentFocus>midFocus) // Nearer than halfway
    		   {
    			   lens->setFocus(nearFocus);
    			   stepDir = -1; // Move focus away
    		   }
    		   else
    		   {
    			   lens->setFocus(farFocus);
    			   stepDir = 1; // Move focus nearer
    		   }

    	   }
    		   break;
    	   case MYAUTOFOCUS_SWEEP:
    		   break;
    	   case MYAUTOFOCUS_DONE:
    		   break;
    	   }
       }

       void update(const FCam::Frame &f) {
    	   /* [CS478]
    	    * This method is supposed to be called in order to inform
    	    * the autofocus engine of a new viewfinder frame.
    	    * You probably want to compute how sharp it is, and possibly
    	    * use the information to plan where to position the lens next.
    	    */
    	   switch(state)
    	   {
    	   case MYAUTOFOCUS_READY:
    		   break;

    	   case MYAUTOFOCUS_SWEEP:
    		   if(numSteps<MaxSteps)
    		   {
    			   // Grab raw data of a frame, compute sharpness and store. Higher is better
    			   float s = computeSharpness(f.image());

    			   foci[numSteps] = (float)f["lens.focus"];
    			   scores[numSteps] = s;
    			   numSteps++;

    			   // Check the new score against best observed score so far to see
    			   //	if we should stop early
    			   if(s>bestScore)
    				   bestScore = s;
    			   else
    				   if((numSteps>(MaxSteps/2)) && (s<0.8f*bestScore)) // We're in a losing curve (presumably)
    					   state = MYAUTOFOCUS_DONE; // Don't stop early if we're in color mode

    			   // Otherwise, simple linear scan of the focal length
    			   float nextFocus = foci[numSteps-1] + stepDir*df;

    			   // Set next focus
    			   lens->setFocus(nextFocus);
    		   }
    		   else
    			   state = MYAUTOFOCUS_DONE; // This sweep instance is out of steps.

    		   break;

    	   case MYAUTOFOCUS_DONE:

			   bestFocusIdx = 1; // Note that we skip index 0 which seems to glitch often
			   for(int idx=1; idx<numSteps; idx++)
				   if(scores[idx]>scores[bestFocusIdx])
					   bestFocusIdx = idx;

			   lens->setFocus(foci[bestFocusIdx]);
			   LOG("THK: MyAutoFocus done. numSteps %d Focus %.3f cm Sharpness %.3f\n",
					   numSteps,1.0f/foci[bestFocusIdx]*100.0f,scores[bestFocusIdx]);
			   state = MYAUTOFOCUS_READY;
    		   break;

    	   }
       }
private:

       float computeSharpness(const FCam::Image &i)
       {
    	   int iw = i.width();
    	   int ih = i.height();

    	   int w, h;
    	   w = (rect.width)  ? rect.width  : iw; // If there is a nonzero width in 'rect' then overwrite
    	   h = (rect.height) ? rect.height : ih;
    	   int xmax = ((rect.x+w)>iw) ? iw : (rect.x+w); // Don't touch memory not belonging to us
    	   int ymax = ((rect.y+h)>ih) ? ih : (rect.y+h);

    	   const int stepSize   = (rect.width) ? 1 : 10; // To speed up the calculation for full frame

    	   float diff = 0.0f;
    	   float sum  = 0.0f;
    	   unsigned char* src = i(0,0);

    	   for(unsigned int y = rect.y+1; y<(ymax-1); y+=stepSize)
    		   for(unsigned int x = rect.x+1; x<(xmax-1); x+=stepSize)
    		   {
    			   // These are all Y values for sharpness computation
    			   unsigned char* center = src + y*PREVIEW_IMAGE_WIDTH + x;
    			   unsigned char* up 	 = center - PREVIEW_IMAGE_WIDTH;
    			   unsigned char* down	 = center + PREVIEW_IMAGE_WIDTH;
    			   unsigned char* left 	 = center - 1;
    			   unsigned char* right  = center + 1;

    			   // Chrominance values
//    			   unsigned char* u = PI_U_OFFSET + src + (y/2)*PREVIEW_IMAGE_WIDTH + (x/2);
//    			   unsigned char* v = (PI_V_OFFSET-PI_U_OFFSET) + u;

    			   // Compute the average of UDLR gradients
				   diff += 0.25f*(abs(*up  	 - *center) +
								  abs(*down  - *center) +
								  abs(*left  - *center) +
								  abs(*right - *center));

				   // Rudimentary normalization of intensity
				   sum += *center;
    		   }

    	   return (sum<1.0f) ? 0 : diff/sum*100.0f;
       }

       FCam::Tegra::Lens* lens;
       FCam::Rect rect;
       /* [CS478]
        * Declare any state variables you might need here.
        */

       int state;
       int numSteps;
       float stepDir;
       float farFocus, nearFocus, df;

       int bestFocusIdx;
       float bestScore;
       float foci[MaxSteps];
       float scores[MaxSteps];
};

#endif
