guest/hals/camera/fake-pipeline2/Sensor.cpp - device/google/cuttlefish_common - Git at Google

 /*
  * Copyright (C) 2012 The Android Open Source Project
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
  *      http://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */

 //#define LOG_NDEBUG 0
 //#define LOG_NNDEBUG 0
 #define LOG_TAG "EmulatedCamera2_Sensor"

 #ifdef LOG_NNDEBUG
 #define ALOGVV(...) ALOGV(__VA_ARGS__)
 #else
 #define ALOGVV(...) ((void)0)
 #endif

 #include <utils/Log.h>

 #include <cmath>
 #include <cstdlib>
 #include "../EmulatedFakeCamera2.h"
 #include "Sensor.h"
 #include "system/camera_metadata.h"

 namespace android {

 // const nsecs_t Sensor::kExposureTimeRange[2] =
 //    {1000L, 30000000000L} ; // 1 us - 30 sec
 // const nsecs_t Sensor::kFrameDurationRange[2] =
 //    {33331760L, 30000000000L}; // ~1/30 s - 30 sec
 const nsecs_t Sensor::kExposureTimeRange[2] = {1000L,
                                                300000000L};  // 1 us - 0.3 sec
 const nsecs_t Sensor::kFrameDurationRange[2] = {
     33331760L, 300000000L};  // ~1/30 s - 0.3 sec

 const nsecs_t Sensor::kMinVerticalBlank = 10000L;

 const uint8_t Sensor::kColorFilterArrangement =
     ANDROID_SENSOR_INFO_COLOR_FILTER_ARRANGEMENT_RGGB;

 // Output image data characteristics
 const uint32_t Sensor::kMaxRawValue = 4000;
 const uint32_t Sensor::kBlackLevel = 1000;

 // Sensor sensitivity
 const float Sensor::kSaturationVoltage = 0.520f;
 const uint32_t Sensor::kSaturationElectrons = 2000;
 const float Sensor::kVoltsPerLuxSecond = 0.100f;

 const float Sensor::kElectronsPerLuxSecond = Sensor::kSaturationElectrons /
                                              Sensor::kSaturationVoltage *
                                              Sensor::kVoltsPerLuxSecond;

 const float Sensor::kBaseGainFactor =
     (float)Sensor::kMaxRawValue / Sensor::kSaturationElectrons;

 const float Sensor::kReadNoiseStddevBeforeGain = 1.177;  // in electrons
 const float Sensor::kReadNoiseStddevAfterGain = 2.100;   // in digital counts
 const float Sensor::kReadNoiseVarBeforeGain =
     Sensor::kReadNoiseStddevBeforeGain * Sensor::kReadNoiseStddevBeforeGain;
 const float Sensor::kReadNoiseVarAfterGain =
     Sensor::kReadNoiseStddevAfterGain * Sensor::kReadNoiseStddevAfterGain;

 const int32_t Sensor::kSensitivityRange[2] = {100, 1600};
 const uint32_t Sensor::kDefaultSensitivity = 100;

 /** A few utility functions for math, normal distributions */

 // Take advantage of IEEE floating-point format to calculate an approximate
 // square root. Accurate to within +-3.6%
 float sqrtf_approx(float r) {
   // Modifier is based on IEEE floating-point representation; the
   // manipulations boil down to finding approximate log2, dividing by two, and
   // then inverting the log2. A bias is added to make the relative error
   // symmetric about the real answer.
   const int32_t modifier = 0x1FBB4000;

   int32_t r_i = *(int32_t *)(&r);
   r_i = (r_i >> 1) + modifier;

   return *(float *)(&r_i);
 }

 Sensor::Sensor(uint32_t width, uint32_t height)
     : Thread(false),
       mResolution{width, height},
       mActiveArray{0, 0, width, height},
       mRowReadoutTime(kFrameDurationRange[0] / height),
       mGotVSync(false),
       mExposureTime(kFrameDurationRange[0] - kMinVerticalBlank),
       mFrameDuration(kFrameDurationRange[0]),
       mGainFactor(kDefaultSensitivity),
       mNextBuffers(NULL),
       mFrameNumber(0),
       mCapturedBuffers(NULL),
       mListener(NULL),
       mScene(width, height, kElectronsPerLuxSecond) {
   ALOGV("Sensor created with pixel array %d x %d", width, height);
 }

 Sensor::~Sensor() { shutDown(); }

 status_t Sensor::startUp() {
   ALOGV("%s: E", __FUNCTION__);

   int res;
   mCapturedBuffers = NULL;
   res = run("EmulatedFakeCamera2::Sensor", ANDROID_PRIORITY_URGENT_DISPLAY);

   if (res != OK) {
     ALOGE("Unable to start up sensor capture thread: %d", res);
   }
   return res;
 }

 status_t Sensor::shutDown() {
   ALOGV("%s: E", __FUNCTION__);

   int res;
   res = requestExitAndWait();
   if (res != OK) {
     ALOGE("Unable to shut down sensor capture thread: %d", res);
   }
   return res;
 }

 Scene &Sensor::getScene() { return mScene; }

 void Sensor::setExposureTime(uint64_t ns) {
   Mutex::Autolock lock(mControlMutex);
   ALOGVV("Exposure set to %f", ns / 1000000.f);
   mExposureTime = ns;
 }

 void Sensor::setFrameDuration(uint64_t ns) {
   Mutex::Autolock lock(mControlMutex);
   ALOGVV("Frame duration set to %f", ns / 1000000.f);
   mFrameDuration = ns;
 }

 void Sensor::setSensitivity(uint32_t gain) {
   Mutex::Autolock lock(mControlMutex);
   ALOGVV("Gain set to %d", gain);
   mGainFactor = gain;
 }

 void Sensor::setDestinationBuffers(Buffers *buffers) {
   Mutex::Autolock lock(mControlMutex);
   mNextBuffers = buffers;
 }

 void Sensor::setFrameNumber(uint32_t frameNumber) {
   Mutex::Autolock lock(mControlMutex);
   mFrameNumber = frameNumber;
 }

 bool Sensor::waitForVSync(nsecs_t reltime) {
   int res;
   Mutex::Autolock lock(mControlMutex);

   mGotVSync = false;
   res = mVSync.waitRelative(mControlMutex, reltime);
   if (res != OK && res != TIMED_OUT) {
     ALOGE("%s: Error waiting for VSync signal: %d", __FUNCTION__, res);
     return false;
   }
   return mGotVSync;
 }

 bool Sensor::waitForNewFrame(nsecs_t reltime, nsecs_t *captureTime) {
   Mutex::Autolock lock(mReadoutMutex);

   if (mCapturedBuffers == NULL) {
     int res;
     res = mReadoutAvailable.waitRelative(mReadoutMutex, reltime);
     if (res == TIMED_OUT) {
       return false;
     } else if (res != OK || mCapturedBuffers == NULL) {
       ALOGE("Error waiting for sensor readout signal: %d", res);
       return false;
     }
   }
   mReadoutComplete.signal();

   *captureTime = mCaptureTime;
   mCapturedBuffers = NULL;
   return true;
 }

 Sensor::SensorListener::~SensorListener() {}

 void Sensor::setSensorListener(SensorListener *listener) {
   Mutex::Autolock lock(mControlMutex);
   mListener = listener;
 }

 status_t Sensor::readyToRun() {
   ALOGV("Starting up sensor thread");
   mStartupTime = systemTime();
   mNextCaptureTime = 0;
   mNextCapturedBuffers = NULL;
   return OK;
 }

 bool Sensor::threadLoop() {
   /**
    * Sensor capture operation main loop.
    *
    * Stages are out-of-order relative to a single frame's processing, but
    * in-order in time.
    */

   /**
    * Stage 1: Read in latest control parameters
    */
   uint64_t exposureDuration;
   uint64_t frameDuration;
   uint32_t gain;
   Buffers *nextBuffers;
   uint32_t frameNumber;
   SensorListener *listener = NULL;
   {
     Mutex::Autolock lock(mControlMutex);
     exposureDuration = mExposureTime;
     frameDuration = mFrameDuration;
     gain = mGainFactor;
     nextBuffers = mNextBuffers;
     frameNumber = mFrameNumber;
     listener = mListener;
     // Don't reuse a buffer set
     mNextBuffers = NULL;

     // Signal VSync for start of readout
     ALOGVV("Sensor VSync");
     mGotVSync = true;
     mVSync.signal();
   }

   /**
    * Stage 3: Read out latest captured image
    */

   Buffers *capturedBuffers = NULL;
   nsecs_t captureTime = 0;

   nsecs_t startRealTime = systemTime();
   // Stagefright cares about system time for timestamps, so base simulated
   // time on that.
   nsecs_t simulatedTime = startRealTime;
   nsecs_t frameEndRealTime = startRealTime + frameDuration;

   if (mNextCapturedBuffers != NULL) {
     ALOGVV("Sensor starting readout");
     // Pretend we're doing readout now; will signal once enough time has elapsed
     capturedBuffers = mNextCapturedBuffers;
     captureTime = mNextCaptureTime;
   }
   simulatedTime += mRowReadoutTime + kMinVerticalBlank;

   // TODO: Move this signal to another thread to simulate readout
   // time properly
   if (capturedBuffers != NULL) {
     ALOGVV("Sensor readout complete");
     Mutex::Autolock lock(mReadoutMutex);
     if (mCapturedBuffers != NULL) {
       ALOGV("Waiting for readout thread to catch up!");
       mReadoutComplete.wait(mReadoutMutex);
     }

     mCapturedBuffers = capturedBuffers;
     mCaptureTime = captureTime;
     mReadoutAvailable.signal();
     capturedBuffers = NULL;
   }

   /**
    * Stage 2: Capture new image
    */
   mNextCaptureTime = simulatedTime;
   mNextCapturedBuffers = nextBuffers;

   if (mNextCapturedBuffers != NULL) {
     if (listener != NULL) {
       listener->onSensorEvent(frameNumber, SensorListener::EXPOSURE_START,
                               mNextCaptureTime);
     }
     ALOGVV("Starting next capture: Exposure: %f ms, gain: %d",
            (float)exposureDuration / 1e6, gain);
     mScene.setExposureDuration((float)exposureDuration / 1e9);
     mScene.calculateScene(mNextCaptureTime);

     // Might be adding more buffers, so size isn't constant
     for (size_t i = 0; i < mNextCapturedBuffers->size(); i++) {
       const StreamBuffer &b = (*mNextCapturedBuffers)[i];
       ALOGVV(
           "Sensor capturing buffer %d: stream %d,"
           " %d x %d, format %x, stride %d, buf %p, img %p",
           i, b.streamId, b.width, b.height, b.format, b.stride, b.buffer,
           b.img);
       switch (b.format) {
         case HAL_PIXEL_FORMAT_RAW16:
           captureRaw(b.img, gain, b.stride);
           break;
         case HAL_PIXEL_FORMAT_RGB_888:
           captureRGB(b.img, gain, b.stride);
           break;
         case HAL_PIXEL_FORMAT_RGBA_8888:
           captureRGBA(b.img, gain, b.stride);
           break;
         case HAL_PIXEL_FORMAT_BLOB:
 #if defined HAL_DATASPACE_DEPTH
           if (b.dataSpace != HAL_DATASPACE_DEPTH) {
 #endif
             // Add auxillary buffer of the right size
             // Assumes only one BLOB (JPEG) buffer in
             // mNextCapturedBuffers
             StreamBuffer bAux;
             bAux.streamId = 0;
             bAux.width = b.width;
             bAux.height = b.height;
             bAux.format = HAL_PIXEL_FORMAT_RGB_888;
             bAux.stride = b.width;
             bAux.buffer = NULL;
             // TODO: Reuse these
             bAux.img = new uint8_t[b.width * b.height * 3];
             mNextCapturedBuffers->push_back(bAux);
 #if defined HAL_DATASPACE_DEPTH
           } else {
             captureDepthCloud(b.img);
           }
 #endif
           break;
         case HAL_PIXEL_FORMAT_YCrCb_420_SP:
         case HAL_PIXEL_FORMAT_YCbCr_420_888:
           captureNV21(b.img, gain, b.stride);
           break;
         case HAL_PIXEL_FORMAT_YV12:
           // TODO:
           ALOGE("%s: Format %x is TODO", __FUNCTION__, b.format);
           break;
         case HAL_PIXEL_FORMAT_Y16:
           captureDepth(b.img, gain, b.stride);
           break;
         default:
           ALOGE("%s: Unknown format %x, no output", __FUNCTION__, b.format);
           break;
       }
     }
   }

   ALOGVV("Sensor vertical blanking interval");
   nsecs_t workDoneRealTime = systemTime();
   const nsecs_t timeAccuracy = 2e6;  // 2 ms of imprecision is ok
   if (workDoneRealTime < frameEndRealTime - timeAccuracy) {
     timespec t;
     t.tv_sec = (frameEndRealTime - workDoneRealTime) / 1000000000L;
     t.tv_nsec = (frameEndRealTime - workDoneRealTime) % 1000000000L;

     int ret;
     do {
       ret = nanosleep(&t, &t);
     } while (ret != 0);
   }
   nsecs_t endRealTime __unused = systemTime();
   ALOGVV("Frame cycle took %d ms, target %d ms",
          (int)((endRealTime - startRealTime) / 1000000),
          (int)(frameDuration / 1000000));
   return true;
 };

 void Sensor::captureRaw(uint8_t *img, uint32_t gain, uint32_t stride) {
   float totalGain = gain / 100.0 * kBaseGainFactor;
   float noiseVarGain = totalGain * totalGain;
   float readNoiseVar =
       kReadNoiseVarBeforeGain * noiseVarGain + kReadNoiseVarAfterGain;

   int bayerSelect[4] = {Scene::R, Scene::Gr, Scene::Gb, Scene::B};  // RGGB
   mScene.setReadoutPixel(0, 0);
   for (unsigned int y = 0; y < mResolution[1]; y++) {
     int *bayerRow = bayerSelect + (y & 0x1) * 2;
     uint16_t *px = (uint16_t *)img + y * stride;
     for (unsigned int x = 0; x < mResolution[0]; x++) {
       uint32_t electronCount;
       electronCount = mScene.getPixelElectrons()[bayerRow[x & 0x1]];

       // TODO: Better pixel saturation curve?
       electronCount = (electronCount < kSaturationElectrons)
                           ? electronCount
                           : kSaturationElectrons;

       // TODO: Better A/D saturation curve?
       uint16_t rawCount = electronCount * totalGain;
       rawCount = (rawCount < kMaxRawValue) ? rawCount : kMaxRawValue;

       // Calculate noise value
       // TODO: Use more-correct Gaussian instead of uniform noise
       float photonNoiseVar = electronCount * noiseVarGain;
       float noiseStddev = sqrtf_approx(readNoiseVar + photonNoiseVar);
       // Scaled to roughly match gaussian/uniform noise stddev
       float noiseSample = std::rand() * (2.5 / (1.0 + RAND_MAX)) - 1.25;

       rawCount += kBlackLevel;
       rawCount += noiseStddev * noiseSample;

       *px++ = rawCount;
     }
     // TODO: Handle this better
     // simulatedTime += mRowReadoutTime;
   }
   ALOGVV("Raw sensor image captured");
 }

 void Sensor::captureRGBA(uint8_t *img, uint32_t gain, uint32_t stride) {
   float totalGain = gain / 100.0 * kBaseGainFactor;
   // In fixed-point math, calculate total scaling from electrons to 8bpp
   int scale64x = 64 * totalGain * 255 / kMaxRawValue;
   uint32_t inc = ceil((float)mResolution[0] / stride);

   for (unsigned int y = 0, outY = 0; y < mResolution[1]; y += inc, outY++) {
     uint8_t *px = img + outY * stride * 4;
     mScene.setReadoutPixel(0, y);
     for (unsigned int x = 0; x < mResolution[0]; x += inc) {
       uint32_t rCount, gCount, bCount;
       // TODO: Perfect demosaicing is a cheat
       const uint32_t *pixel = mScene.getPixelElectrons();
       rCount = pixel[Scene::R] * scale64x;
       gCount = pixel[Scene::Gr] * scale64x;
       bCount = pixel[Scene::B] * scale64x;

       *px++ = rCount < 255 * 64 ? rCount / 64 : 255;
       *px++ = gCount < 255 * 64 ? gCount / 64 : 255;
       *px++ = bCount < 255 * 64 ? bCount / 64 : 255;
       *px++ = 255;
       for (unsigned int j = 1; j < inc; j++) mScene.getPixelElectrons();
     }
     // TODO: Handle this better
     // simulatedTime += mRowReadoutTime;
   }
   ALOGVV("RGBA sensor image captured");
 }

 void Sensor::captureRGB(uint8_t *img, uint32_t gain, uint32_t stride) {
   float totalGain = gain / 100.0 * kBaseGainFactor;
   // In fixed-point math, calculate total scaling from electrons to 8bpp
   int scale64x = 64 * totalGain * 255 / kMaxRawValue;
   uint32_t inc = ceil((float)mResolution[0] / stride);

   for (unsigned int y = 0, outY = 0; y < mResolution[1]; y += inc, outY++) {
     mScene.setReadoutPixel(0, y);
     uint8_t *px = img + outY * stride * 3;
     for (unsigned int x = 0; x < mResolution[0]; x += inc) {
       uint32_t rCount, gCount, bCount;
       // TODO: Perfect demosaicing is a cheat
       const uint32_t *pixel = mScene.getPixelElectrons();
       rCount = pixel[Scene::R] * scale64x;
       gCount = pixel[Scene::Gr] * scale64x;
       bCount = pixel[Scene::B] * scale64x;

       *px++ = rCount < 255 * 64 ? rCount / 64 : 255;
       *px++ = gCount < 255 * 64 ? gCount / 64 : 255;
       *px++ = bCount < 255 * 64 ? bCount / 64 : 255;
       for (unsigned int j = 1; j < inc; j++) mScene.getPixelElectrons();
     }
     // TODO: Handle this better
     // simulatedTime += mRowReadoutTime;
   }
   ALOGVV("RGB sensor image captured");
 }

 void Sensor::captureNV21(uint8_t *img, uint32_t gain, uint32_t stride) {
   float totalGain = gain / 100.0 * kBaseGainFactor;
   // Using fixed-point math with 6 bits of fractional precision.
   // In fixed-point math, calculate total scaling from electrons to 8bpp
   const int scale64x = 64 * totalGain * 255 / kMaxRawValue;
   // In fixed-point math, saturation point of sensor after gain
   const int saturationPoint = 64 * 255;
   // Fixed-point coefficients for RGB-YUV transform
   // Based on JFIF RGB->YUV transform.
   // Cb/Cr offset scaled by 64x twice since they're applied post-multiply
   const int rgbToY[] = {19, 37, 7};
   const int rgbToCb[] = {-10, -21, 32, 524288};
   const int rgbToCr[] = {32, -26, -5, 524288};
   // Scale back to 8bpp non-fixed-point
   const int scaleOut = 64;
   const int scaleOutSq = scaleOut * scaleOut;  // after multiplies

   // inc = how many pixels to skip while reading every next pixel
   // horizontally.
   uint32_t inc = ceil((float)mResolution[0] / stride);
   // outH = projected vertical resolution based on stride.
   uint32_t outH = mResolution[1] / inc;
   for (unsigned int y = 0, outY = 0; y < mResolution[1]; y += inc, outY++) {
     uint8_t *pxY = img + outY * stride;
     uint8_t *pxVU = img + (outH + outY / 2) * stride;
     mScene.setReadoutPixel(0, y);
     for (unsigned int outX = 0; outX < stride; outX++) {
       int32_t rCount, gCount, bCount;
       // TODO: Perfect demosaicing is a cheat
       const uint32_t *pixel = mScene.getPixelElectrons();
       rCount = pixel[Scene::R] * scale64x;
       rCount = rCount < saturationPoint ? rCount : saturationPoint;
       gCount = pixel[Scene::Gr] * scale64x;
       gCount = gCount < saturationPoint ? gCount : saturationPoint;
       bCount = pixel[Scene::B] * scale64x;
       bCount = bCount < saturationPoint ? bCount : saturationPoint;

       *pxY++ = (rgbToY[0] * rCount + rgbToY[1] * gCount + rgbToY[2] * bCount) /
                scaleOutSq;
       if (outY % 2 == 0 && outX % 2 == 0) {
         *pxVU++ = (rgbToCb[0] * rCount + rgbToCb[1] * gCount +
                    rgbToCb[2] * bCount + rgbToCb[3]) /
                   scaleOutSq;
         *pxVU++ = (rgbToCr[0] * rCount + rgbToCr[1] * gCount +
                    rgbToCr[2] * bCount + rgbToCr[3]) /
                   scaleOutSq;
       }

       // Skip unprocessed pixels from sensor.
       for (unsigned int j = 1; j < inc; j++) mScene.getPixelElectrons();
     }
   }
   ALOGVV("NV21 sensor image captured");
 }

 void Sensor::captureDepth(uint8_t *img, uint32_t gain, uint32_t stride) {
   float totalGain = gain / 100.0 * kBaseGainFactor;
   // In fixed-point math, calculate scaling factor to 13bpp millimeters
   int scale64x = 64 * totalGain * 8191 / kMaxRawValue;
   uint32_t inc = ceil((float)mResolution[0] / stride);

   for (unsigned int y = 0, outY = 0; y < mResolution[1]; y += inc, outY++) {
     mScene.setReadoutPixel(0, y);
     uint16_t *px = ((uint16_t *)img) + outY * stride;
     for (unsigned int x = 0; x < mResolution[0]; x += inc) {
       uint32_t depthCount;
       // TODO: Make up real depth scene instead of using green channel
       // as depth
       const uint32_t *pixel = mScene.getPixelElectrons();
       depthCount = pixel[Scene::Gr] * scale64x;

       *px++ = depthCount < 8191 * 64 ? depthCount / 64 : 0;
       for (unsigned int j = 1; j < inc; j++) mScene.getPixelElectrons();
     }
     // TODO: Handle this better
     // simulatedTime += mRowReadoutTime;
   }
   ALOGVV("Depth sensor image captured");
 }

 void Sensor::captureDepthCloud(uint8_t * /*img*/) {
 #if defined HAL_DATASPACE_DEPTH
   android_depth_points *cloud = reinterpret_cast<android_depth_points *>(img);

   cloud->num_points = 16;

   // TODO: Create point cloud values that match RGB scene
   const int FLOATS_PER_POINT = 4;
   const float JITTER_STDDEV = 0.1f;
   for (size_t y = 0, i = 0; y < 4; y++) {
     for (size_t x = 0; x < 4; x++, i++) {
       float randSampleX = std::rand() * (2.5f / (1.0f + RAND_MAX)) - 1.25f;
       randSampleX *= JITTER_STDDEV;

       float randSampleY = std::rand() * (2.5f / (1.0f + RAND_MAX)) - 1.25f;
       randSampleY *= JITTER_STDDEV;

       float randSampleZ = std::rand() * (2.5f / (1.0f + RAND_MAX)) - 1.25f;
       randSampleZ *= JITTER_STDDEV;

       cloud->xyzc_points[i * FLOATS_PER_POINT + 0] = x - 1.5f + randSampleX;
       cloud->xyzc_points[i * FLOATS_PER_POINT + 1] = y - 1.5f + randSampleY;
       cloud->xyzc_points[i * FLOATS_PER_POINT + 2] = 3.f + randSampleZ;
       cloud->xyzc_points[i * FLOATS_PER_POINT + 3] = 0.8f;
     }
   }

   ALOGVV("Depth point cloud captured");
 #endif
 }

 }  // namespace android
	/*
	* Copyright (C) 2012 The Android Open Source Project
	*
	* Licensed under the Apache License, Version 2.0 (the "License");
	* you may not use this file except in compliance with the License.
	* You may obtain a copy of the License at
	*
	* http://www.apache.org/licenses/LICENSE-2.0
	*
	* Unless required by applicable law or agreed to in writing, software
	* distributed under the License is distributed on an "AS IS" BASIS,
	* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
	* See the License for the specific language governing permissions and
	* limitations under the License.
	*/

	//#define LOG_NDEBUG 0
	//#define LOG_NNDEBUG 0
	#define LOG_TAG "EmulatedCamera2_Sensor"

	#ifdef LOG_NNDEBUG
	#define ALOGVV(...) ALOGV(__VA_ARGS__)
	#else
	#define ALOGVV(...) ((void)0)
	#endif

	#include <utils/Log.h>

	#include <cmath>
	#include <cstdlib>
	#include "../EmulatedFakeCamera2.h"
	#include "Sensor.h"
	#include "system/camera_metadata.h"

	namespace android {

	// const nsecs_t Sensor::kExposureTimeRange[2] =
	// {1000L, 30000000000L} ; // 1 us - 30 sec
	// const nsecs_t Sensor::kFrameDurationRange[2] =
	// {33331760L, 30000000000L}; // ~1/30 s - 30 sec
	const nsecs_t Sensor::kExposureTimeRange[2] = {1000L,
	300000000L}; // 1 us - 0.3 sec
	const nsecs_t Sensor::kFrameDurationRange[2] = {
	33331760L, 300000000L}; // ~1/30 s - 0.3 sec

	const nsecs_t Sensor::kMinVerticalBlank = 10000L;

	const uint8_t Sensor::kColorFilterArrangement =
	ANDROID_SENSOR_INFO_COLOR_FILTER_ARRANGEMENT_RGGB;

	// Output image data characteristics
	const uint32_t Sensor::kMaxRawValue = 4000;
	const uint32_t Sensor::kBlackLevel = 1000;

	// Sensor sensitivity
	const float Sensor::kSaturationVoltage = 0.520f;
	const uint32_t Sensor::kSaturationElectrons = 2000;
	const float Sensor::kVoltsPerLuxSecond = 0.100f;

	const float Sensor::kElectronsPerLuxSecond = Sensor::kSaturationElectrons /
	Sensor::kSaturationVoltage *
	Sensor::kVoltsPerLuxSecond;

	const float Sensor::kBaseGainFactor =
	(float)Sensor::kMaxRawValue / Sensor::kSaturationElectrons;

	const float Sensor::kReadNoiseStddevBeforeGain = 1.177; // in electrons
	const float Sensor::kReadNoiseStddevAfterGain = 2.100; // in digital counts
	const float Sensor::kReadNoiseVarBeforeGain =
	Sensor::kReadNoiseStddevBeforeGain * Sensor::kReadNoiseStddevBeforeGain;
	const float Sensor::kReadNoiseVarAfterGain =
	Sensor::kReadNoiseStddevAfterGain * Sensor::kReadNoiseStddevAfterGain;

	const int32_t Sensor::kSensitivityRange[2] = {100, 1600};
	const uint32_t Sensor::kDefaultSensitivity = 100;

	/** A few utility functions for math, normal distributions */

	// Take advantage of IEEE floating-point format to calculate an approximate
	// square root. Accurate to within +-3.6%
	float sqrtf_approx(float r) {
	// Modifier is based on IEEE floating-point representation; the
	// manipulations boil down to finding approximate log2, dividing by two, and
	// then inverting the log2. A bias is added to make the relative error
	// symmetric about the real answer.
	const int32_t modifier = 0x1FBB4000;

	int32_t r_i = (int32_t )(&r);
	r_i = (r_i >> 1) + modifier;

	return (float )(&r_i);
	}

	Sensor::Sensor(uint32_t width, uint32_t height)
	: Thread(false),
	mResolution{width, height},
	mActiveArray{0, 0, width, height},
	mRowReadoutTime(kFrameDurationRange[0] / height),
	mGotVSync(false),
	mExposureTime(kFrameDurationRange[0] - kMinVerticalBlank),
	mFrameDuration(kFrameDurationRange[0]),
	mGainFactor(kDefaultSensitivity),
	mNextBuffers(NULL),
	mFrameNumber(0),
	mCapturedBuffers(NULL),
	mListener(NULL),
	mScene(width, height, kElectronsPerLuxSecond) {
	ALOGV("Sensor created with pixel array %d x %d", width, height);
	}

	Sensor::~Sensor() { shutDown(); }

	status_t Sensor::startUp() {
	ALOGV("%s: E", __FUNCTION__);

	int res;
	mCapturedBuffers = NULL;
	res = run("EmulatedFakeCamera2::Sensor", ANDROID_PRIORITY_URGENT_DISPLAY);

	if (res != OK) {
	ALOGE("Unable to start up sensor capture thread: %d", res);
	}
	return res;
	}

	status_t Sensor::shutDown() {
	ALOGV("%s: E", __FUNCTION__);

	int res;
	res = requestExitAndWait();
	if (res != OK) {
	ALOGE("Unable to shut down sensor capture thread: %d", res);
	}
	return res;
	}

	Scene &Sensor::getScene() { return mScene; }

	void Sensor::setExposureTime(uint64_t ns) {
	Mutex::Autolock lock(mControlMutex);
	ALOGVV("Exposure set to %f", ns / 1000000.f);
	mExposureTime = ns;
	}

	void Sensor::setFrameDuration(uint64_t ns) {
	Mutex::Autolock lock(mControlMutex);
	ALOGVV("Frame duration set to %f", ns / 1000000.f);
	mFrameDuration = ns;
	}

	void Sensor::setSensitivity(uint32_t gain) {
	Mutex::Autolock lock(mControlMutex);
	ALOGVV("Gain set to %d", gain);
	mGainFactor = gain;
	}

	void Sensor::setDestinationBuffers(Buffers *buffers) {
	Mutex::Autolock lock(mControlMutex);
	mNextBuffers = buffers;
	}

	void Sensor::setFrameNumber(uint32_t frameNumber) {
	Mutex::Autolock lock(mControlMutex);
	mFrameNumber = frameNumber;
	}

	bool Sensor::waitForVSync(nsecs_t reltime) {
	int res;
	Mutex::Autolock lock(mControlMutex);

	mGotVSync = false;
	res = mVSync.waitRelative(mControlMutex, reltime);
	if (res != OK && res != TIMED_OUT) {
	ALOGE("%s: Error waiting for VSync signal: %d", __FUNCTION__, res);
	return false;
	}
	return mGotVSync;
	}

	bool Sensor::waitForNewFrame(nsecs_t reltime, nsecs_t *captureTime) {
	Mutex::Autolock lock(mReadoutMutex);

	if (mCapturedBuffers == NULL) {
	int res;
	res = mReadoutAvailable.waitRelative(mReadoutMutex, reltime);
	if (res == TIMED_OUT) {
	return false;
	} else if (res != OK \|\| mCapturedBuffers == NULL) {
	ALOGE("Error waiting for sensor readout signal: %d", res);
	return false;
	}
	}
	mReadoutComplete.signal();

	*captureTime = mCaptureTime;
	mCapturedBuffers = NULL;
	return true;
	}

	Sensor::SensorListener::~SensorListener() {}

	void Sensor::setSensorListener(SensorListener *listener) {
	Mutex::Autolock lock(mControlMutex);
	mListener = listener;
	}

	status_t Sensor::readyToRun() {
	ALOGV("Starting up sensor thread");
	mStartupTime = systemTime();
	mNextCaptureTime = 0;
	mNextCapturedBuffers = NULL;
	return OK;
	}

	bool Sensor::threadLoop() {
	/**
	* Sensor capture operation main loop.
	*
	* Stages are out-of-order relative to a single frame's processing, but
	* in-order in time.
	*/

	/**
	* Stage 1: Read in latest control parameters
	*/
	uint64_t exposureDuration;
	uint64_t frameDuration;
	uint32_t gain;
	Buffers *nextBuffers;
	uint32_t frameNumber;
	SensorListener *listener = NULL;
	{
	Mutex::Autolock lock(mControlMutex);
	exposureDuration = mExposureTime;
	frameDuration = mFrameDuration;
	gain = mGainFactor;
	nextBuffers = mNextBuffers;
	frameNumber = mFrameNumber;
	listener = mListener;
	// Don't reuse a buffer set
	mNextBuffers = NULL;

	// Signal VSync for start of readout
	ALOGVV("Sensor VSync");
	mGotVSync = true;
	mVSync.signal();
	}

	/**
	* Stage 3: Read out latest captured image
	*/

	Buffers *capturedBuffers = NULL;
	nsecs_t captureTime = 0;

	nsecs_t startRealTime = systemTime();
	// Stagefright cares about system time for timestamps, so base simulated
	// time on that.
	nsecs_t simulatedTime = startRealTime;
	nsecs_t frameEndRealTime = startRealTime + frameDuration;

	if (mNextCapturedBuffers != NULL) {
	ALOGVV("Sensor starting readout");
	// Pretend we're doing readout now; will signal once enough time has elapsed
	capturedBuffers = mNextCapturedBuffers;
	captureTime = mNextCaptureTime;
	}
	simulatedTime += mRowReadoutTime + kMinVerticalBlank;

	// TODO: Move this signal to another thread to simulate readout
	// time properly
	if (capturedBuffers != NULL) {
	ALOGVV("Sensor readout complete");
	Mutex::Autolock lock(mReadoutMutex);
	if (mCapturedBuffers != NULL) {
	ALOGV("Waiting for readout thread to catch up!");
	mReadoutComplete.wait(mReadoutMutex);
	}

	mCapturedBuffers = capturedBuffers;
	mCaptureTime = captureTime;
	mReadoutAvailable.signal();
	capturedBuffers = NULL;
	}

	/**
	* Stage 2: Capture new image
	*/
	mNextCaptureTime = simulatedTime;
	mNextCapturedBuffers = nextBuffers;

	if (mNextCapturedBuffers != NULL) {
	if (listener != NULL) {
	listener->onSensorEvent(frameNumber, SensorListener::EXPOSURE_START,
	mNextCaptureTime);
	}
	ALOGVV("Starting next capture: Exposure: %f ms, gain: %d",
	(float)exposureDuration / 1e6, gain);
	mScene.setExposureDuration((float)exposureDuration / 1e9);
	mScene.calculateScene(mNextCaptureTime);

	// Might be adding more buffers, so size isn't constant
	for (size_t i = 0; i < mNextCapturedBuffers->size(); i++) {
	const StreamBuffer &b = (*mNextCapturedBuffers)[i];
	ALOGVV(
	"Sensor capturing buffer %d: stream %d,"
	" %d x %d, format %x, stride %d, buf %p, img %p",
	i, b.streamId, b.width, b.height, b.format, b.stride, b.buffer,
	b.img);
	switch (b.format) {
	case HAL_PIXEL_FORMAT_RAW16:
	captureRaw(b.img, gain, b.stride);
	break;
	case HAL_PIXEL_FORMAT_RGB_888:
	captureRGB(b.img, gain, b.stride);
	break;
	case HAL_PIXEL_FORMAT_RGBA_8888:
	captureRGBA(b.img, gain, b.stride);
	break;
	case HAL_PIXEL_FORMAT_BLOB:
	#if defined HAL_DATASPACE_DEPTH
	if (b.dataSpace != HAL_DATASPACE_DEPTH) {
	#endif
	// Add auxillary buffer of the right size
	// Assumes only one BLOB (JPEG) buffer in
	// mNextCapturedBuffers
	StreamBuffer bAux;
	bAux.streamId = 0;
	bAux.width = b.width;
	bAux.height = b.height;
	bAux.format = HAL_PIXEL_FORMAT_RGB_888;
	bAux.stride = b.width;
	bAux.buffer = NULL;
	// TODO: Reuse these
	bAux.img = new uint8_t[b.width * b.height * 3];
	mNextCapturedBuffers->push_back(bAux);
	#if defined HAL_DATASPACE_DEPTH
	} else {
	captureDepthCloud(b.img);
	}
	#endif
	break;
	case HAL_PIXEL_FORMAT_YCrCb_420_SP:
	case HAL_PIXEL_FORMAT_YCbCr_420_888:
	captureNV21(b.img, gain, b.stride);
	break;
	case HAL_PIXEL_FORMAT_YV12:
	// TODO:
	ALOGE("%s: Format %x is TODO", __FUNCTION__, b.format);
	break;
	case HAL_PIXEL_FORMAT_Y16:
	captureDepth(b.img, gain, b.stride);
	break;
	default:
	ALOGE("%s: Unknown format %x, no output", __FUNCTION__, b.format);
	break;
	}
	}
	}

	ALOGVV("Sensor vertical blanking interval");
	nsecs_t workDoneRealTime = systemTime();
	const nsecs_t timeAccuracy = 2e6; // 2 ms of imprecision is ok
	if (workDoneRealTime < frameEndRealTime - timeAccuracy) {
	timespec t;
	t.tv_sec = (frameEndRealTime - workDoneRealTime) / 1000000000L;
	t.tv_nsec = (frameEndRealTime - workDoneRealTime) % 1000000000L;

	int ret;
	do {
	ret = nanosleep(&t, &t);
	} while (ret != 0);
	}
	nsecs_t endRealTime __unused = systemTime();
	ALOGVV("Frame cycle took %d ms, target %d ms",
	(int)((endRealTime - startRealTime) / 1000000),
	(int)(frameDuration / 1000000));
	return true;
	};

	void Sensor::captureRaw(uint8_t *img, uint32_t gain, uint32_t stride) {
	float totalGain = gain / 100.0 * kBaseGainFactor;
	float noiseVarGain = totalGain * totalGain;
	float readNoiseVar =
	kReadNoiseVarBeforeGain * noiseVarGain + kReadNoiseVarAfterGain;

	int bayerSelect[4] = {Scene::R, Scene::Gr, Scene::Gb, Scene::B}; // RGGB
	mScene.setReadoutPixel(0, 0);
	for (unsigned int y = 0; y < mResolution[1]; y++) {
	int bayerRow = bayerSelect + (y & 0x1) 2;
	uint16_t px = (uint16_t )img + y * stride;
	for (unsigned int x = 0; x < mResolution[0]; x++) {
	uint32_t electronCount;
	electronCount = mScene.getPixelElectrons()[bayerRow[x & 0x1]];

	// TODO: Better pixel saturation curve?
	electronCount = (electronCount < kSaturationElectrons)
	? electronCount
	: kSaturationElectrons;

	// TODO: Better A/D saturation curve?
	uint16_t rawCount = electronCount * totalGain;
	rawCount = (rawCount < kMaxRawValue) ? rawCount : kMaxRawValue;

	// Calculate noise value
	// TODO: Use more-correct Gaussian instead of uniform noise
	float photonNoiseVar = electronCount * noiseVarGain;
	float noiseStddev = sqrtf_approx(readNoiseVar + photonNoiseVar);
	// Scaled to roughly match gaussian/uniform noise stddev
	float noiseSample = std::rand() * (2.5 / (1.0 + RAND_MAX)) - 1.25;

	rawCount += kBlackLevel;
	rawCount += noiseStddev * noiseSample;

	*px++ = rawCount;
	}
	// TODO: Handle this better
	// simulatedTime += mRowReadoutTime;
	}
	ALOGVV("Raw sensor image captured");
	}

	void Sensor::captureRGBA(uint8_t *img, uint32_t gain, uint32_t stride) {
	float totalGain = gain / 100.0 * kBaseGainFactor;
	// In fixed-point math, calculate total scaling from electrons to 8bpp
	int scale64x = 64 * totalGain * 255 / kMaxRawValue;
	uint32_t inc = ceil((float)mResolution[0] / stride);

	for (unsigned int y = 0, outY = 0; y < mResolution[1]; y += inc, outY++) {
	uint8_t px = img + outY stride * 4;
	mScene.setReadoutPixel(0, y);
	for (unsigned int x = 0; x < mResolution[0]; x += inc) {
	uint32_t rCount, gCount, bCount;
	// TODO: Perfect demosaicing is a cheat
	const uint32_t *pixel = mScene.getPixelElectrons();
	rCount = pixel[Scene::R] * scale64x;
	gCount = pixel[Scene::Gr] * scale64x;
	bCount = pixel[Scene::B] * scale64x;

	px++ = rCount < 255 64 ? rCount / 64 : 255;
	px++ = gCount < 255 64 ? gCount / 64 : 255;
	px++ = bCount < 255 64 ? bCount / 64 : 255;
	*px++ = 255;
	for (unsigned int j = 1; j < inc; j++) mScene.getPixelElectrons();
	}
	// TODO: Handle this better
	// simulatedTime += mRowReadoutTime;
	}
	ALOGVV("RGBA sensor image captured");
	}

	void Sensor::captureRGB(uint8_t *img, uint32_t gain, uint32_t stride) {
	float totalGain = gain / 100.0 * kBaseGainFactor;
	// In fixed-point math, calculate total scaling from electrons to 8bpp
	int scale64x = 64 * totalGain * 255 / kMaxRawValue;
	uint32_t inc = ceil((float)mResolution[0] / stride);

	for (unsigned int y = 0, outY = 0; y < mResolution[1]; y += inc, outY++) {
	mScene.setReadoutPixel(0, y);
	uint8_t px = img + outY stride * 3;
	for (unsigned int x = 0; x < mResolution[0]; x += inc) {
	uint32_t rCount, gCount, bCount;
	// TODO: Perfect demosaicing is a cheat
	const uint32_t *pixel = mScene.getPixelElectrons();
	rCount = pixel[Scene::R] * scale64x;
	gCount = pixel[Scene::Gr] * scale64x;
	bCount = pixel[Scene::B] * scale64x;

	px++ = rCount < 255 64 ? rCount / 64 : 255;
	px++ = gCount < 255 64 ? gCount / 64 : 255;
	px++ = bCount < 255 64 ? bCount / 64 : 255;
	for (unsigned int j = 1; j < inc; j++) mScene.getPixelElectrons();
	}
	// TODO: Handle this better
	// simulatedTime += mRowReadoutTime;
	}
	ALOGVV("RGB sensor image captured");
	}

	void Sensor::captureNV21(uint8_t *img, uint32_t gain, uint32_t stride) {
	float totalGain = gain / 100.0 * kBaseGainFactor;
	// Using fixed-point math with 6 bits of fractional precision.
	// In fixed-point math, calculate total scaling from electrons to 8bpp
	const int scale64x = 64 * totalGain * 255 / kMaxRawValue;
	// In fixed-point math, saturation point of sensor after gain
	const int saturationPoint = 64 * 255;
	// Fixed-point coefficients for RGB-YUV transform
	// Based on JFIF RGB->YUV transform.
	// Cb/Cr offset scaled by 64x twice since they're applied post-multiply
	const int rgbToY[] = {19, 37, 7};
	const int rgbToCb[] = {-10, -21, 32, 524288};
	const int rgbToCr[] = {32, -26, -5, 524288};
	// Scale back to 8bpp non-fixed-point
	const int scaleOut = 64;
	const int scaleOutSq = scaleOut * scaleOut; // after multiplies

	// inc = how many pixels to skip while reading every next pixel
	// horizontally.
	uint32_t inc = ceil((float)mResolution[0] / stride);
	// outH = projected vertical resolution based on stride.
	uint32_t outH = mResolution[1] / inc;
	for (unsigned int y = 0, outY = 0; y < mResolution[1]; y += inc, outY++) {
	uint8_t pxY = img + outY stride;
	uint8_t pxVU = img + (outH + outY / 2) stride;
	mScene.setReadoutPixel(0, y);
	for (unsigned int outX = 0; outX < stride; outX++) {
	int32_t rCount, gCount, bCount;
	// TODO: Perfect demosaicing is a cheat
	const uint32_t *pixel = mScene.getPixelElectrons();
	rCount = pixel[Scene::R] * scale64x;
	rCount = rCount < saturationPoint ? rCount : saturationPoint;
	gCount = pixel[Scene::Gr] * scale64x;
	gCount = gCount < saturationPoint ? gCount : saturationPoint;
	bCount = pixel[Scene::B] * scale64x;
	bCount = bCount < saturationPoint ? bCount : saturationPoint;

	pxY++ = (rgbToY[0] rCount + rgbToY[1] * gCount + rgbToY[2] * bCount) /
	scaleOutSq;
	if (outY % 2 == 0 && outX % 2 == 0) {
	pxVU++ = (rgbToCb[0] rCount + rgbToCb[1] * gCount +
	rgbToCb[2] * bCount + rgbToCb[3]) /
	scaleOutSq;
	pxVU++ = (rgbToCr[0] rCount + rgbToCr[1] * gCount +
	rgbToCr[2] * bCount + rgbToCr[3]) /
	scaleOutSq;
	}

	// Skip unprocessed pixels from sensor.
	for (unsigned int j = 1; j < inc; j++) mScene.getPixelElectrons();
	}
	}
	ALOGVV("NV21 sensor image captured");
	}

	void Sensor::captureDepth(uint8_t *img, uint32_t gain, uint32_t stride) {
	float totalGain = gain / 100.0 * kBaseGainFactor;
	// In fixed-point math, calculate scaling factor to 13bpp millimeters
	int scale64x = 64 * totalGain * 8191 / kMaxRawValue;
	uint32_t inc = ceil((float)mResolution[0] / stride);

	for (unsigned int y = 0, outY = 0; y < mResolution[1]; y += inc, outY++) {
	mScene.setReadoutPixel(0, y);
	uint16_t px = ((uint16_t )img) + outY * stride;
	for (unsigned int x = 0; x < mResolution[0]; x += inc) {
	uint32_t depthCount;
	// TODO: Make up real depth scene instead of using green channel
	// as depth
	const uint32_t *pixel = mScene.getPixelElectrons();
	depthCount = pixel[Scene::Gr] * scale64x;

	px++ = depthCount < 8191 64 ? depthCount / 64 : 0;
	for (unsigned int j = 1; j < inc; j++) mScene.getPixelElectrons();
	}
	// TODO: Handle this better
	// simulatedTime += mRowReadoutTime;
	}
	ALOGVV("Depth sensor image captured");
	}

	void Sensor::captureDepthCloud(uint8_t * /img/) {
	#if defined HAL_DATASPACE_DEPTH
	android_depth_points cloud = reinterpret_cast<android_depth_points >(img);

	cloud->num_points = 16;

	// TODO: Create point cloud values that match RGB scene
	const int FLOATS_PER_POINT = 4;
	const float JITTER_STDDEV = 0.1f;
	for (size_t y = 0, i = 0; y < 4; y++) {
	for (size_t x = 0; x < 4; x++, i++) {
	float randSampleX = std::rand() * (2.5f / (1.0f + RAND_MAX)) - 1.25f;
	randSampleX *= JITTER_STDDEV;

	float randSampleY = std::rand() * (2.5f / (1.0f + RAND_MAX)) - 1.25f;
	randSampleY *= JITTER_STDDEV;

	float randSampleZ = std::rand() * (2.5f / (1.0f + RAND_MAX)) - 1.25f;
	randSampleZ *= JITTER_STDDEV;

	cloud->xyzc_points[i * FLOATS_PER_POINT + 0] = x - 1.5f + randSampleX;
	cloud->xyzc_points[i * FLOATS_PER_POINT + 1] = y - 1.5f + randSampleY;
	cloud->xyzc_points[i * FLOATS_PER_POINT + 2] = 3.f + randSampleZ;
	cloud->xyzc_points[i * FLOATS_PER_POINT + 3] = 0.8f;
	}
	}

	ALOGVV("Depth point cloud captured");
	#endif
	}

	} // namespace android