//========================================================================
// SRLib robotics library
//
// AUTHOR:  Gabor Nagy
// DATE:    2018-Apr-01 00:00:47
//
// Copyright (C) 2014-2026 By Skyline Robotics. All rights reserved.
//========================================================================
#ifndef _SR_Encoder_h
#define _SR_Encoder_h

#ifdef __BEAGLEBONE__
#include <SR/BeagleBone.h>
#endif

#if defined (__cplusplus) && (!(defined (__CPLUSLIBS)))
extern "C" {
#endif


typedef struct
{
	int64_t  PulsesPerRev;
	EBool    HasIndex;

	int64_t  Position;
	int64_t  LastPositionAtIndex;
	uint8_t  PrevState;

#ifdef __BEAGLEBONE__
// GPIO addresses GPIO port * 32 + pin (e.g. GPIO1_6 = 32 * 1 + 6 = 38)
// -1 : not used / invalid
//
	int16_t  Abit, Bbit, Ibit;
#endif
} SrEncoder;


#define SrEncChA      0x01
#define SrEncChB      0x02
#define SrEncChINDEX  0x04

#if EIndex_BITS == 64
#define SrPosNOT_SET  0xFFFFFFFFFFFFFFFF
#else
#define SrPosNOT_SET  0xFFFFFFFF
#endif

enum
{
	SrEncCW = 1,
	SrEncCCW,
	SrEncERROR
};


/*
Quadrature rotary encoder codes
Phase 	A 	B  Code
  1     0   0    0
  2     0   1    2
  3     1   1    3
  4     1   0    1
	
Coding for
counter-clockwise rotation
Phase 	A 	B  Code 
  1     1   0    1
  2     1   1    3
  3     0   1    2
  4     0   0    0
*/

// Look-up table for quadrature encoder transitions
// [PrevState][State]  A = bit0  B = bit1
// 0: no change
static const uint8_t  Sr_EncoderChangeCodes[4][4]=
{
	{          0,   SrEncCCW,    SrEncCW, SrEncERROR },
	{    SrEncCW,          0, SrEncERROR,   SrEncCCW },
	{   SrEncCCW, SrEncERROR,          0,    SrEncCW },
	{ SrEncERROR,    SrEncCW,   SrEncCCW,          0 }
};


static inline uint8_t Sr_EncoderGetCode(const uint8_t PPrevState, const uint8_t PState)
{
	return(Sr_EncoderChangeCodes[PPrevState & (SrEncChA|SrEncChB)][PState & (SrEncChA|SrEncChB)]);
}


static inline int Sr_EncoderAction(SrEncoder* PEncoder, const uint8_t PState)
{
	int  LResult = E_SUCCESS;

	if(PEncoder->PrevState != PState)
	{
//printf("%s() %s:%d  PrevState %02x  PState %02x\n", __FUNCTION__, __FILE__, __LINE__, PEncoder->PrevState, PState);fflush(stdout);
		printf("%s() %s:%d ", __FUNCTION__, __FILE__, __LINE__);fflush(stdout);
		if(PEncoder->PrevState & SrEncChA) printf("%sA%s", ECodeGreen, ECodeNormal);else printf(" ");
		if(PEncoder->PrevState & SrEncChB) printf("%sB%s", ECodeGreen, ECodeNormal);else printf(" ");
		if(PEncoder->PrevState & SrEncChINDEX) printf("%sI%s", ECodeYellow, ECodeNormal);else printf(" ");
printf(" -> ");fflush(stdout);
		if(PState & SrEncChA) printf("%sA%s", ECodeGreen, ECodeNormal);else printf(" ");
		if(PState & SrEncChB) printf("%sB%s", ECodeGreen, ECodeNormal);else printf(" ");
		if(PState & SrEncChINDEX) printf("%sI%s", ECodeYellow, ECodeNormal);else printf(" ");

		const uint8_t  LCode = Sr_EncoderGetCode(PEncoder->PrevState, PState);
		switch(LCode)
		{
			case SrEncCW:   PEncoder->Position++;break;
			case SrEncCCW:  PEncoder->Position--;break;
			case SrEncERROR:
printf("%sERROR reading encoder%s (%02x -> %02x)", ECodeRed, ECodeNormal, PEncoder->PrevState & (SrEncChA|SrEncChB), PState & (SrEncChA|SrEncChB));
//assert(0);
				LResult = E_CORRUPT_DATA;
			break;
		}


		PEncoder->PrevState = PState;

		if(PState & SrEncChINDEX)
		{
			if(PEncoder->LastPositionAtIndex == SrPosNOT_SET)
			{
			}
		}


printf("% lld\n", (long long)PEncoder->Position);fflush(stdout);
	}
	else LResult = E_NO_CHANGE;

	return(LResult);
}


static inline void Sr_EncoderInit(SrEncoder* PEncoder)
{
//int32_t PPulsesPerRev, const EBool PHasIndex, const uint8_t PState
	PEncoder->PulsesPerRev = 0;
	PEncoder->HasIndex     = FALSE;

	PEncoder->Position     = 0;
	PEncoder->LastPositionAtIndex = SrPosNOT_SET;
	PEncoder->PrevState = 0;

#ifdef __BEAGLEBONE__
	PEncoder->Abit = -1;PEncoder->Bbit = -1;PEncoder->Ibit = -1;
#endif
}


static inline void Sr_EncoderConnect(SrEncoder* PEncoder)
{
#ifdef __BEAGLEBONE__
	Sr_SetToIn(PEncoder->A);
	Sr_SetToIn(PEncoder->B);
	Sr_SetToIn(PEncoder->I);
#endif
}


#if defined (__cplusplus) && (!(defined (__CPLUSLIBS)))
}	// Close scope of 'extern "C"' declaration
#endif

#endif	// _SR_Encoder_h
