//========================================================================
// SRLib robotics library
//
// AUTHOR:  Gabor Nagy
// DATE:    2024-Dec-12 21:05:23
//
// Copyright (C) 2014-2026 By Skyline Robotics. All rights reserved.
//========================================================================
#ifndef _SR_Motor_h
#define _SR_Motor_h

#include <EClass.h>

#include <SR/Encoder.h>

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


extern int  Sr_UDPPortNumber, Sr_TCPPortNumber;


#define SrFloat  float
#define Sr_FAbs  fabsf

/*
#define SrFloat  float
#define Sr_FAbs  fabs
*/


// Motion-profile segment
//
typedef struct
{
	ETime    StartMicroseconds, PlannedTime;
	EIndex   EndPosition;
	SrFloat  StartingSpeed, Acceleration, TargetSpeed;
} SrMotionSegment;


typedef struct
{
	SrMotionSegment  Segments[8];int  Segments_Count;
	int              SegmentIndex;
} SrMotionProfile;


#ifdef __BEAGLEBONE__
#include <SR/BeagleBone.h>
#define Sr_GPIOInit(m, mVerbosity)  _BeagleBoneGPIOInit(m, mVerbosity)
#else
#include <SR/IOStub.h>
#define Sr_GPIOInit(m, mVerbosity)
#endif


typedef struct SrMotorClass_S        SrMotorClass;          // Forward-define SrMotorClass
typedef struct SrMotorDriverClass_S  SrMotorDriverClass;    // Forward-define SrMotorDriverClass
typedef struct SrControllerClass_S   SrControllerClass;     // Forward-define SrMotorDriverClass



// Motor and MotorDrriver classes
// These represent an abstraction for motors and motor driver hardware.
// They "hide" where the driver is physically: it may be directly connected to the host computer, or it may be remote via ethernet, USB etc.
// E.g. a robot's lower arm's motors may be controlled by one microcontroller (e.g. BeagleBoard), the upper arm's may be controlled by another one.
// Yet, the motor and driver may be accessed transparently through these classes, even after "hot" un-plugging and re-plugging into another microcontroller.
// This allows for redundancy (e.g. if a microcontroller fails, just plug its motors into another one's free ports) and simper high-level motion control code.
//
#ifdef __BEAGLEBONE__
#define BeagleBoneFields  SrGPIO*     GPIO;
#else
#define BeagleBoneFields
#endif

#define SrMotorDriverCore\
	EObjectCore(SrMotorDriver);\
	EBool       ThreadRun;\
	BeagleBoneFields\
	SrEncoder   Encoder


typedef struct
{
	SrMotorDriverCore;
}	SrMotorDriver;


struct SrMotorDriverClass_S
{
	EBaseClassCore(SrMotorDriver);

	void  (*Connect)(SrMotorDriver* PDriver, SrGPIO* PGPIO);  // Connect driver to the hardware (e.g. BeagleBone GPIO)
};




static inline void Sr_MotorDriverInit(SrMotorDriver* PDriver)
{
	PDriver->ThreadRun = FALSE;
#ifdef __BEAGLEBONE__
	PDriver->GPIO = NULL;
#endif
	Sr_EncoderInit(&PDriver->Encoder);
}


// Motor status flags
//
#define  SrMotorStatusSOFTLIMIT_MIN       0x00000001  // Hit soft limit (Motor->PositionMin)
#define  SrMotorStatusSOFTLIMIT_MAX       0x00000002  // Hit soft limit (Motor->PositionMin)
#define  SrMotorStatusHARDLIMIT_MIN       0x00000004  // Hit hard limit (encoder is not recording movement)
#define  SrMotorStatusHARDLIMIT_MAX       0x00000008  // Hit hard limit (encoder is not recording movement)
#define  SrMotorStatusLIMITSENSOR_MIN     0x00000010  // Hit physical limit sensor
#define  SrMotorStatusLIMITSENSOR_MAX     0x00000020  // Hit physical limit sensor
//#define  SrMotorStatusMECHANICAL_SLIP_FORWARD   0x00000040  // Encoder is recording movement, but can't reach limit sensor
//#define  SrMotorStatusMECHANICAL_SLIP_BACKWARD  0x00000080  // Encoder is recording movement, but can't reach limit sensor
#define  SrMotorStatusDRIVER_FAULT        0x00000F00  // Driver circuit faults
#define  SrMotorStatusDRIVER_OVERHEAT     0x00000100
#define  SrMotorStatusDRIVER_OVERCURRENT  0x00000200
#define  SrMotorStatusDRIVER_POWER_LOSS   0x00000400
#define  SrMotorStatusMOTOR_OVERHEAT      0x00001000


#define SrMotorCore\
	EObjectCore(SrMotor);\
	SrMotorDriver*   Driver;\
	int64_t          TargetPosition;\
	int64_t          PositionMin, PositionMax;\
	float            Speed, Acceleration, Deceleration, SpeedMax;\
	int64_t          Position, EncoderPosition;\
	int16_t          Power_Holding, Power_Max;   /* Percentage */\
	int8_t           Direction;                  /* -1, 0 or 1            */\
	uint32_t         StatusFlags;                /* SrMotorLIMIT_MIN etc. */\
	SrMotionProfile  MotionProfile

typedef struct
{
	SrMotorCore;
} SrMotor;


struct SrMotorClass_S
{
	EBaseClassCore(SrMotor);
};

typedef struct
{
	SrMotorCore;
} SrStepperMotor;



static inline void Sr_MotorDriverConnect(SrMotor* PMotor, SrGPIO* PGPIO)
{
	SrMotorDriver*  LDriver = PMotor->Driver;

	LDriver->Class->Connect(LDriver, PGPIO);
}



#define SrControllerCore\
	EObjectCore(SrController);\
	EList(, SrMotor*, Motors)


typedef struct
{
	SrControllerCore;
}	SrController;

struct SrControllerClass_S
{
	EBaseClassCore(SrController);
};



extern const SrMotorClass  SrMotor_class[];
extern const SrMotorClass  SrStepperMotor_class[];

extern const SrControllerClass  SrController_class[];

EList(extern, SrMotorDriverClass*, Sr_MotorDriverClasses);

extern SrMotorDriverClass*  SrMotorDriver_ClassByName(const char* PName);
extern SrMotorDriverClass*  SrMotorDriver_ClassRegisterInactive(const char* PName);
extern void                 SrMotorDriverClasses_Free(void);


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

#endif	// _SR_Motor_h
