/***********************************************************************************************************************
*                                                                                                                      *
* libscopeprotocols                                                                                                    *
*                                                                                                                      *
* Copyright (c) 2012-2022 Andrew D. Zonenberg and contributors                                                         *
* All rights reserved.                                                                                                 *
*                                                                                                                      *
* Redistribution and use in source and binary forms, with or without modification, are permitted provided that the     *
* following conditions are met:                                                                                        *
*                                                                                                                      *
*    * Redistributions of source code must retain the above copyright notice, this list of conditions, and the         *
*      following disclaimer.                                                                                           *
*                                                                                                                      *
*    * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the       *
*      following disclaimer in the documentation and/or other materials provided with the distribution.                *
*                                                                                                                      *
*    * Neither the name of the author nor the names of any contributors may be used to endorse or promote products     *
*      derived from this software without specific prior written permission.                                           *
*                                                                                                                      *
* THIS SOFTWARE IS PROVIDED BY THE AUTHORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED   *
* TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL *
* THE AUTHORS BE HELD LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES        *
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR       *
* BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT *
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE       *
* POSSIBILITY OF SUCH DAMAGE.                                                                                          *
*                                                                                                                      *
***********************************************************************************************************************/

#include "../scopehal/scopehal.h"
#include "QuadratureDecoder.h"

using namespace std;

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Construction / destruction

QuadratureDecoder::QuadratureDecoder(const string& color)
	: Filter(color, CAT_MISC)
{
	AddStream(Unit(Unit::UNIT_DEGREES), "data", Stream::STREAM_TYPE_ANALOG);
	CreateInput("A");
	CreateInput("B");
	//CreateInput("Reset");

	m_pulseratename = "Pulses per rev";
	m_parameters[m_pulseratename] = FilterParameter(FilterParameter::TYPE_INT, Unit(Unit::UNIT_COUNTS));
	m_parameters[m_pulseratename].SetFloatVal(0);

	m_interpname = "Interpolation";
	m_parameters[m_interpname] = FilterParameter(FilterParameter::TYPE_ENUM, Unit(Unit::UNIT_COUNTS));
	m_parameters[m_interpname].AddEnumValue("None", INTERP_NONE);
	m_parameters[m_interpname].AddEnumValue("Linear", INTERP_LINEAR);

	m_revname = "Revolutions";
	m_parameters[m_revname] = FilterParameter(FilterParameter::TYPE_ENUM, Unit(Unit::UNIT_COUNTS));
	m_parameters[m_revname].AddEnumValue("Single", MODE_SINGLE_REV);
	m_parameters[m_revname].AddEnumValue("Multi", MODE_MULTI_REV);

	m_debouncename = "Debounce Cooldown";
	m_parameters[m_debouncename] = FilterParameter(FilterParameter::TYPE_INT, Unit(Unit::UNIT_FS));
	m_parameters[m_debouncename].ParseString("1 ms");
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Factory methods

bool QuadratureDecoder::ValidateChannel(size_t i, StreamDescriptor stream)
{
	if(stream.m_channel == NULL)
		return false;

	if( (i < 2) && (stream.GetType() == Stream::STREAM_TYPE_DIGITAL) )
		return true;

	return false;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Accessors

string QuadratureDecoder::GetProtocolName()
{
	return "Quadrature";
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Actual decoder logic

void QuadratureDecoder::Refresh()
{
	//Make sure we've got valid inputs
	if(!VerifyAllInputsOK())
	{
		SetData(NULL, 0);
		return;
	}

	auto a = GetInputWaveform(0);
	auto b = GetInputWaveform(1);
	a->PrepareForCpuAccess();
	b->PrepareForCpuAccess();

	auto sa = dynamic_cast<SparseDigitalWaveform*>(a);
	auto sb = dynamic_cast<SparseDigitalWaveform*>(b);
	auto ua = dynamic_cast<UniformDigitalWaveform*>(a);
	auto ub = dynamic_cast<UniformDigitalWaveform*>(b);

	float phase_per_pulse = 360 / m_parameters[m_pulseratename].GetFloatVal();

	InterpolationMode mode = (InterpolationMode)m_parameters[m_interpname].GetIntVal();
	RevMode rmode = (RevMode)m_parameters[m_revname].GetIntVal();
	int64_t debounce_fs = m_parameters[m_debouncename].GetIntVal();
	int64_t debounce_samples = debounce_fs / a->m_timescale;

	//Create the output waveform
	auto cap = SetupEmptySparseAnalogOutputWaveform(a, 0);
	cap->PrepareForCpuAccess();

	//Seed with initial point at time zero
	int64_t last_edge = 0;
	cap->m_offsets.push_back(0);
	cap->m_durations.push_back(1);
	cap->m_samples.push_back(0);

	int64_t timestamp = 0;
	size_t ia = 0;
	size_t ib = 0;
	size_t alen = a->size();
	size_t blen = b->size();

	float phase = 0;

	enum
	{
		STATE_BOTH_HIGH,
		STATE_A_HIGH,
		STATE_BOTH_LOW,
		STATE_B_HIGH
	} state = STATE_BOTH_LOW;

	if(GetValue(sa, ua, 0) && GetValue(sb, ub, 0))
		state = STATE_BOTH_HIGH;

	while(true)
	{
		bool ca = GetValue(sa, ua, ia);
		bool cb = GetValue(sb, ub, ib);

		//TODO: add mode to say look for both edges or only rising

		//Lagging phase
		size_t ilast = cap->size() - 1;
		bool edge = false;
		bool phase_positive = true;

		//Ignore toggles for a user-specified time after another toggle
		int64_t samples_since_edge = (timestamp - last_edge);
		if( samples_since_edge >= debounce_samples)
		{
			switch(state)
			{
				//Both signals are low. Look for a rising edge
				case STATE_BOTH_LOW:

					//A is leading
					if(ca)
					{
						phase_positive = false;
						edge = true;
						state = STATE_A_HIGH;
					}

					//A is lagging
					if(cb)
					{
						phase_positive = true;
						edge = true;
						state = STATE_B_HIGH;
					}

					break;

				//Ignore edges until the other signal toggles
				case STATE_A_HIGH:
					if(cb)
						state = STATE_BOTH_HIGH;
					if(!ca)
						state = STATE_BOTH_LOW;
					break;
				case STATE_B_HIGH:
					if(ca)
						state = STATE_BOTH_HIGH;
					if(!cb)
						state = STATE_BOTH_LOW;
					break;

				//Both are high, look for falling edges
				case STATE_BOTH_HIGH:

					//A is leading
					if(!ca)
					{
						phase_positive = false;
						//edge = true;
						state = STATE_B_HIGH;
					}

					//A is lagging
					if(!cb)
					{
						phase_positive = true;
						//edge = true;
						state = STATE_A_HIGH;
					}

					break;
			}
		}

		//Add samples if we get a pulse
		if(edge)
		{
			last_edge = timestamp;

			if(phase_positive)
			{
				phase += phase_per_pulse;
				if(rmode == MODE_SINGLE_REV)
				{
					if(phase > 180)
						phase -= 360;
				}
			}
			else
			{
				phase -= phase_per_pulse;
				if(rmode == MODE_MULTI_REV)
				{
					if(phase < -180)
						phase += 360;
				}
			}

			if(mode == INTERP_LINEAR)
			{
				//Update duration of last pulse
				cap->m_durations[ilast] = timestamp - last_edge;

				//Add the new sample
				cap->m_offsets.push_back(timestamp);
				cap->m_durations.push_back(0);
				cap->m_samples.push_back(phase);
			}
			else
			{
				//Update duration of last pulse
				cap->m_durations[ilast] = timestamp - last_edge - 1;

				//Add a new sample for the edge
				cap->m_offsets.push_back(timestamp-1);
				cap->m_durations.push_back(1);
				cap->m_samples.push_back(cap->m_samples[ilast]);

				//Add a new sample for the point
				cap->m_offsets.push_back(timestamp);
				cap->m_durations.push_back(0);
				cap->m_samples.push_back(phase);
			}
		}

		//Get timestamps of next event on each channel.
		//If we can't move forward, stop.
		int64_t next_a = GetNextEventTimestamp(sa, ua, ia, alen, timestamp);
		int64_t next_b = GetNextEventTimestamp(sb, ub, ib, blen, timestamp);
		int64_t next_timestamp = min(next_a, next_b);
		if(next_timestamp == timestamp)
			break;
		timestamp = next_timestamp;
		AdvanceToTimestamp(sa, ua, ia, alen, timestamp);
		AdvanceToTimestamp(sb, ub, ib, blen, timestamp);
	}

	//If less than 2 samples, stop
	if(cap->m_durations.size() < 2)
	{
		delete cap;
		SetData(NULL, 0);
		return;
	}

	//Extend last sample
	size_t ilast = cap->m_durations.size() - 1;
	cap->m_durations[ilast] = timestamp - last_edge;

	SetData(cap, 0);
}
