#include <stdio.h>
#include "libmegapixels.h"


float
clamp_float(float value, float min, float max)
{
	if (value > max)
		return max;

	if (value < min)
		return min;

	return value;
}

void
libmegapixels_aaa_init(libmegapixels_aaa_stats *stats)
{
	float identity[] = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f};
	libmegapixels_aaa_set_matrix(stats, identity, identity);
}

void
libmegapixels_aaa_set_matrix(libmegapixels_aaa_stats *stats, const float matrix1[9], const float matrix2[9])
{
	for (size_t i = 0; i < 9; i++) {
		stats->matrix1[i] = matrix1[i];
		stats->matrix2[i] = matrix2[i];
	}
}

void
libmegapixels_aaa_software_statistics(libmegapixels_aaa_stats *stats, const unsigned int *frame, const int width,
	const int height)
{
	unsigned int bright = 0;
	unsigned int too_bright = 0;
	unsigned int too_dark = 0;
	unsigned int total = width * height;
	unsigned long long sum_r = 0, sum_g = 0, sum_b = 0;
	for (ssize_t p = 0; p < width * height; p++) {
		unsigned int r = (frame[p] >> 0) & 0xff;
		unsigned int g = (frame[p] >> 8) & 0xff;
		unsigned int b = (frame[p] >> 16) & 0xff;
		unsigned int y = (r + g + b) / 3;

		if (y > 220) {
			too_bright++;
		}

		if (y > 180) {
			bright++;
		}
		if (y < 2) {
			too_dark++;
		}

		// Whitebalance on the midrange pixels only
		if (y > 75 && y < 200) {
			sum_r += r;
			sum_g += g;
			sum_b += b;
		}
	}

	// Exposure calculations
	unsigned int p_bright = (bright * 100) / total;
	unsigned int p_too_bright = (too_bright * 100) / total;
	unsigned int p_dark = (too_dark * 100) / total;
	stats->exposure = 0;
	if (p_bright < 1) {
		stats->exposure = 1;
	}
	if (p_too_bright > 8) {
		stats->exposure = -1;
	}
	stats->blacklevel = 0;
	if (p_dark < 1) {
		stats->blacklevel = -1;
	}
	if (p_dark > 3) {
		stats->blacklevel = 1;
	}

	// Whitebalance calculations
	float r = (float) sum_r / total;
	float g = (float) sum_g / total;
	float b = (float) sum_b / total;

	stats->avg_r = (r * stats->matrix1[0]) + (g * stats->matrix1[1]) + (b * stats->matrix1[2]);
	stats->avg_g = (r * stats->matrix1[3]) + (g * stats->matrix1[4]) + (b * stats->matrix1[5]);
	stats->avg_b = (r * stats->matrix1[6]) + (g * stats->matrix1[7]) + (b * stats->matrix1[8]);
}