#define __STDC_LIMIT_MACROS

#include "Plate.hpp"

#include <stdint.h>

#include <cassert>

#include <algorithm>

using std::swap;

inline int32_t do_step(int32_t s, int32_t prev_s, int64_t a, int32_t Dampping, int32_t K, int32_t inv_mass) {
		a = (((a * K) >> 16) * inv_mass) >> 16;
		int64_t v = s - prev_s;
		v = (v * (int64_t)((1 << 16) - Dampping)) >> 16;
		int64_t acc = (int64_t)s + (int64_t)v + (int64_t)a;
		if (acc > INT32_MAX) acc = INT32_MAX;
		if (acc < INT32_MIN) acc = INT32_MIN;
		return acc; //s + v + a;
}


void Plate::step() {
	int32_t *pos_at = pos;
	int32_t *prev_pos_at = prev_pos;
	int32_t *inv_mass_at = inv_mass;
	{ //y == 0
		for (unsigned int x = 0; x < PlateSize; ++x) {
			int32_t s = *pos_at;
			int64_t a = 0;
			if (x > 0) {
				a += *(pos_at - 1) - s;
				a += *(pos_at + PlateSize - 1) - s;
			}
			if (x + 1 < PlateSize) {
				a += *(pos_at + 1) - s;
				a += *(pos_at + PlateSize + 1) - s;
			}
			a += *(pos_at + PlateSize) - s;
			*prev_pos_at = do_step(s,*prev_pos_at, a, Dampping, K,*inv_mass_at);
			++pos_at;
			++prev_pos_at;
			++inv_mass_at;

		}
	} //y == 0
	for (unsigned int y = 1; y+1 < PlateSize; ++y) {
		{ //X == 0
			int32_t s = *pos_at;
			int64_t a = 0;
			a += *(pos_at - PlateSize) - s;
			a += *(pos_at - PlateSize + 1) - s;
			a += *(pos_at + 1) - s;
			a += *(pos_at + PlateSize) - s;
			a += *(pos_at + PlateSize + 1) - s;
			*prev_pos_at = do_step(s,*prev_pos_at, a, Dampping, K,*inv_mass_at);
			++pos_at;
			++prev_pos_at;
			++inv_mass_at;
		} //x == 0
		for (unsigned int x = 1; x+1 < PlateSize; ++x) {
			int32_t s = *pos_at;
			int64_t a = 0;
			a += *(pos_at - PlateSize - 1) - s;
			a += *(pos_at - PlateSize) - s;
			a += *(pos_at - PlateSize + 1) - s;
			a += *(pos_at - 1) - s;
			a += *(pos_at + 1) - s;
			a += *(pos_at + PlateSize - 1) - s;
			a += *(pos_at + PlateSize) - s;
			a += *(pos_at + PlateSize + 1) - s;
			*prev_pos_at = do_step(s,*prev_pos_at, a, Dampping, K,*inv_mass_at);
			++pos_at;
			++prev_pos_at;
			++inv_mass_at;
		}
		{ //X == PlateSize-1
			int32_t s = *pos_at;
			int64_t a = 0;
			a += *(pos_at - PlateSize - 1) - s;
			a += *(pos_at - PlateSize) - s;
			a += *(pos_at - 1) - s;
			a += *(pos_at + PlateSize - 1) - s;
			a += *(pos_at + PlateSize) - s;
			*prev_pos_at = do_step(s,*prev_pos_at, a, Dampping, K,*inv_mass_at);
			++pos_at;
			++prev_pos_at;
			++inv_mass_at;
		} //x == PlateSize-1
	}
	{ //y == PlateSize-1
		for (unsigned int x = 0; x < PlateSize; ++x) {
			int32_t s = *pos_at;
			int64_t a = 0;
			if (x > 0) {
				a += *(pos_at - 1) - s;
				a += *(pos_at - PlateSize - 1) - s;
			}
			if (x + 1 < PlateSize) {
				a += *(pos_at + 1) - s;
				a += *(pos_at - PlateSize + 1) - s;
			}
			a += *(pos_at - PlateSize) - s;
			*prev_pos_at = do_step(s,*prev_pos_at, a, Dampping, K,*inv_mass_at);
			++pos_at;
			++prev_pos_at;
			++inv_mass_at;
		}
	} //y == PlateSize-1

	assert(prev_pos_at == prev_pos + PlateSize*PlateSize);
	assert(pos_at == pos + PlateSize*PlateSize);
	assert(inv_mass_at == inv_mass + PlateSize*PlateSize);

	swap(prev_pos, pos);
}

void Plate::shrink() {
	assert(pos == inv_mass + PlateSize*PlateSize || pos == inv_mass + 2*PlateSize*PlateSize);
	assert(prev_pos == inv_mass + PlateSize*PlateSize || prev_pos == inv_mass + 2*PlateSize*PlateSize);
	//take advantage of our -- somewhat odd -- memory layout:
	for (unsigned int i = PlateSize * PlateSize; i < 3 * PlateSize * PlateSize; ++i) {
		inv_mass[i] = inv_mass[i] >> 1;
	}
}
