// SSonarSensor.h: interface for the SSonar class.
//
//////////////////////////////////////////////////////////////////////

#if !defined(AFX_SSONAR_H__C0C9F306_1816_418C_BE85_A82F7139596A__INCLUDED_)
#define AFX_SSONAR_H__C0C9F306_1816_418C_BE85_A82F7139596A__INCLUDED_

#if _MSC_VER > 1000
#pragma once
#endif // _MSC_VER > 1000

#include "ULinearAlgebra.h"
#include "SQuadTree.h"	// Added by ClassView
#include "D3D8.h"

#define INITIALBEAMWIDTH 5 // 5 cm intial beam width

#define SIMULATORSAMPLING 10 // ten times the normal rate

#define MAXFIRINGRATE 10 // sonar only allowed to fire 10 times a second

#define MAXDISPLAYBUFFER 1000
#define MAXBEAM 300

#define SOUTOFRANGE -5
#define SNOTREADY -1

class SSonarSensor  
{
public: 
	float DirectXGetAngle();

	
	enum THREEPOINTENUM {GotoRight, ScanRight,GotoFoward, ScanFoward, GotoLeft, ScanLeft};
	THREEPOINTENUM ThreePointEnum;
	float TPScanAngle; // 3 degrees scan increment
	float TPScanArc; // 30 degree scan arc
	float TPTempAngle; // temp angle for incrementing
	float TPBeamAngleConst; // different the the normal angle, because of lack of reflection model
	float TPBeamAngleTemp; // temp for holding the old beam angle constant

	// Robot API stuff
	void SNRResetA();
	BOOL SNRIsBlockingA();
	void SNRGet3PointDistanceA(short distance[3]);
	void SNRSetFowardScanA();
	BOOL SNRGetFowardDistanceA(short *Distance);
	void SNRSet3PointScanA();
	float LastDetectionDistance;
	float ThreePointD[3][30];
	int SNRArrayIndex;
	// used for the different modes of sonar commands
	void RunSonarCommand(float &TimeElapse);    

    // the directX rendering function
	void DirectXRender(VERTEX* &pVertex, FPoint RenderModelCenter, int &VertexCount);
	void DirectXFormBeam(VERTEX* pVertex, FPoint Point1, FPoint Point2);
	
    struct BeamObject;

    typedef BeamObject *BeamObjectPtr;
    // Link list structure to store sonar beam information which is a composite
    // of a bunch of lines (points)
    struct BeamObject
    {
        FPoint Start;
        FPoint End;
        BeamObjectPtr Next;
    };

    // Main Sonar Variable Structure
    struct SonarSensorType
    {
        
        float Angle;
        FPoint DisplayPoint;
        FPoint TranslatedDisplayPoint; // coordinated with the simulation
        FPoint TranslatedDisplayPointR; // coodinated with the rendering

        FPoint CurrBeamOrigin; // center of which the beam was emmitted (may not be the current
                                // sonar position though)
        FPoint PrototypeBeamCenter;
        // the prototype for the beam
        BeamObjectPtr BeamObjectPrototype; 
        float InitialBeamScale;

        // Note that because the collision detection needs the current and old
        // Beams two linked list have to be maintained.
        // The prototype beam is used at all times to figure out what the stretched beam looks like

        float BeamAngle; // the angle of the current beam that is shot out
        float CurrentBeamDistance; //the distance of the current beam
    
        // the previous center
        FPoint PreviousBeamTransformedCenter;       
        BeamObjectPtr PreviousTransformedBeam; // the previous beam stretched and traslated to

        // the previous center       
        FPoint CurrentBeamTransformedCenter;     
        BeamObjectPtr CurrentTransformedBeam; // the previous beam stretched and traslated to

        BOOL Valid; // is the sonar loaded?
    };    

    // standard store model info structure
    struct MODELINFO
    {
        #ifdef USEFLOAT
            float x,y; // position        
            float CurrAngle; // Current Angle
        #else
            double x,y; // position        
            double CurrAngle; // Current Angle
        #endif       
        
        FPoint Center;      
    };

	enum SONARCOMMAND {SonarStream,ScanFowards,ThreePointScan};

    struct SonarCommandStruct
    {

        SONARCOMMAND SonarCommand;
		BOOL GotoCommand; // goes to the command area, instead of the rotate/beam simulation area
        BOOL RotateState;
        BOOL ClockWise;

        int StreamAngle;
        float CurrentAngle;
        float DesiredAngle;

        float FireTimeCounter; // keeps track how much time elapse since the last beam fire
    };

    struct SonarCollisionStruct;

    typedef SonarCollisionStruct *SonarCollisionPtr;

    // this structure is used to store past collision points and beams.
    struct SonarCollisionStruct
    {
        int StorageNumber;  // this number is a counter, when it goes to zero the node is nuked
        float Angle;
		float Distance;
        BeamObjectPtr Beam;
        FPoint CollisionPoint;   
        SonarCollisionPtr Next;
        BOOL Valid; // for rendering structure
    };

    struct BeamObjectDisplayStruct
    {
        FPoint Start;
        FPoint End;
    };

     struct SonarCollisionDisplayStruct
    {
        float Angle;
        BeamObjectDisplayStruct Beam[MAXBEAM];
        FPoint CollisionPoint;   
        int NBeams;
        BOOL Valid; // for rendering structure
    };

    // Main Variable
    SonarSensorType SonarSensor; 
    
    // Secondary command variable
    SonarCommandStruct SonarCommand;

    // Linked list for real data because of dynamic data
    SonarCollisionPtr BackBuffer;
    // array for display to avoid linked list style syncronization problems
    SonarCollisionDisplayStruct DisplayBuffer[MAXDISPLAYBUFFER];
    int DisplayBufferIndex; // for the for loop, how many elements are in the display
    BOOL EnableExtraFlipping; // legacy not really used anymore

    // for the primamy display flipping
    BeamObjectDisplayStruct Beam[MAXBEAM];
    int NBeams;

    float MaximumDistance;

    // flips the previous beam logic to render
	void FlipDisplayBuffer();
    // so the previous beam logic could purge old beams
    void RemoveDeadWoodCL();

    // flips the current beam logic to render
    void FlipDisplayBuffer1();

    // makes sure the sonar can't fire faster then the maximum rate
	BOOL CanFireSonar(float &TimeElapse);   

    // quadtree pointer
    SQuadTree *MapPtr;
    
    // Standard model info stuff
    MODELINFO ModelInfo;
    void UpdateModel(MODELINFO ModelInfo);

    // Standard Linear Algebra library
    ULinearAlgebra LAUtils;

    // adds a collision to the collision list for rendering of previous collisions
    void AddToCollisionList(float TimeElapse, FPoint CollisionPoint);

    // stores the old scale so when a collision occurs an easy (not completely perfect)
    // algorithm could be used to position the beam.
    float OldScale1;
	float OldScale;
    

     // The standard GDI Render function call
	void GDIRender(CDC &MemDC, FRect Quad, CRect Screen, float SRScale, FPoint RenderModelCenter);
    // A Render helper function
    BOOL AngleArc2(CDC &MemDC, int X, int Y, DWORD dwRadius,
                  float fStartDegrees, float fSweepDegrees);


    // debug/garbage variables
    BOOL Once;

     // some rendering options
	BOOL RenderPastBeams;
	BOOL RenderPastCollisions;
    int MaxIndexSC;
	void SetRenderOptions(BOOL PastCollision, BOOL PastBeams, int NPastBeamsBOOL, BOOL ExtraFlipping);
    void SetMaximumDistance(float MaximumDistance);
    // options to set variables from other classes
	void SetBeamAngle(float Angle);
	void SetStreamAngle(float Angle);
	void SetSpeedofSound(float Speed);

    
    void TranslateSensor();// Tranlation/rotation for simulation render
    void TranslateSensor(FPoint RenderModelCenter);  // Tranlation/rotation for GDI render

    // Speed and
	float BeamSpeed; // in cm/second
	float BeamAngleConst; // how much the beam spreads out over time (in degrees)
    float StepperRPM; // how fast the stepper motor could rotate

	// Function which simulates the sonar
	void SimulateSonar(float TimeElapse);
     // It might rotate the sonar
	void RotateSonarStream();
	float SonarMod(float Angle);
	float RotateSonar(float TimeElapse);
    // Then alters the beam
    BOOL StretchBeam(float &TimeElapse, BOOL Collision);
    // then checks for the collision
    BOOL CheckforCollision(float &TimeElapse);		

   

    // File load stuff
    void FileChartoLong(long &toRecieve, unsigned char *&ptr);
    // Sensor Loads
    void LoadSonarSensor(CPoint Center);
    void AdjustforCenter(float Left, float Top);
    // Beam Loads
	void LoadSonarBeam(CString FileName);    
    void AdjustforCenterBeam();



	// Deleted the linked lists
	void ResetBeam1();
    void ResetBeam();

	SSonarSensor();
	virtual ~SSonarSensor();

};

#endif // !defined(AFX_SSONAR_H__C0C9F306_1816_418C_BE85_A82F7139596A__INCLUDED_)
