/*
** scan.h
**
** Functions for scans
**
** (c) Steffen Gutmann 1994-99
**
** Creation date: 02.11.94
** Last modified: 17.02.99
*/

#ifndef SCAN_H
#define SCAN_H

#include <stdio.h>
#include "scanstudio/matrix.h"
#include "scanstudio/pos.h"
#include "scanstudio/misc.h"

#ifdef	__cplusplus
extern "C" {
#endif

struct ScanPoint
{
    double sp_X, sp_Y;          /* euclidian absolute position */
    double sp_A, sp_D;		/* polar relative position */
    ulong sp_Num:28;		/* scan point number */
    ulong sp_Status:4;		/* see below */
    short sp_Weight;	        /* number of original points */
    short sp_LineNum;		/* corresponding line number if on line */
};

enum { VISIBLE=0, WRONG_DIRECTION, OCCLUDED, NOT_IN_INTERSECTION };

struct Scan
{
    char sc_Title[101];                         /* string describing scan */
    char sc_Flag;                               /* for various purposes */
    bool sc_Matched;				/* TRUE after sl */
    ulong sc_Id;				/* unique Id */
    struct timeval sc_Timeval;			/* time stamp */
    struct Position sc_Pos;                     /* robots estimated position */
    double sc_Dx, sc_Dy, sc_Da;                 /* scanner pos relative to
						   robots pos system */
    double sc_StartA, sc_EndA;                  /* scanner start and end scan
						   angle position */
    long sc_Num;                                /* number of scan points */
    struct ScanPoint sc_Points[0];
};

bool IsValidScanPoint(struct ScanPoint *sp);
/*
** Returns TRUE if given scan point seems valid
** (range is within a predefined range, etc.) and
** FALSE otherwise.
*/

struct Scan *AllocScan(long num);
/* allocates scan with at most num scan points */

void FreeScan(struct Scan *scan);
/* frees scan */

double DistancePointToPoint(struct ScanPoint *sp1, struct ScanPoint *sp2);
/* calculates euclidian distance from point sp1 to point sp2 */

double AngleDistancePointToPoint(struct ScanPoint *sp1, struct ScanPoint *sp2);
/* calulates angular distance between two points */

double DistanceScanToScan(struct Scan *scan1, struct Scan *scan2);
/* calculates distance between scan positions */

void GetScanPosition(struct Scan *scan, double *sx, double *sy, double *sa);
/* returns absolut scanner position of the given scan in *sx, *sy and *sa */

double ScanFieldOfView(struct Scan *scan);
/* returns field of view of scan in rad. */

void RollScan(struct Scan *scan, double angle);
/*
** Rolls scan (changes order of scan points) such that
** first point lies in direction angle (absolute angle).
*/

double ScanAverageDistance(struct Scan *scan);
/*
** Computes average distance from scanner viewing point
** to scan points.
*/

double ScanAverageDistanceGravity(struct Scan *scan);
/*
** Computes average distance from center of gravity point
** to scan points.
*/

void ScanCenterOfGravity(struct Scan *scan, double *cx, double *cy);
/*
** Returns center of gravity of all scan points in cx, cy. 
*/

double ScanTotalLength(struct Scan *scan);
/*
** Computes surounding length of scan.
*/

double ScanDynamicsGravity(struct Scan *scan);
/* 
** returns difference between maximum and minimum distance of
** scan point to center of gravity.
*/

double ScanAreaSize(struct Scan *scan);
/*
** Returns the size of the visible area in scan.
*/

struct Scan *LoadScan(FILE *file);
/* loads scan from disk with Christian's (old) ascci scan format */

struct Scan *PolarRangesToScan(short num, ushort *r, 
	struct Position *robotpos, struct Position *relpos,
	double start_a, double end_a, bool raw);
/* converts raw range readings to scan structure */

void SaveScan(struct Scan *scan, FILE *file);
/*
** Saves scan in ascii format.
*/

void CopyScan(struct Scan *src, struct Scan *dst);
/* copies src to dest, dest must have enough space for scan points. */

struct Scan *DuplicateScan(struct Scan *src);
/* allocates and copies scan */

void SortScanPoints(struct ScanPoint *sp, long num);
/*
** Sorts scan points in increasing angle.
*/

struct Scan *MergeScans(struct Scan *scans[], int num, 
	struct Position *mergePos);
/*
** Merges a set of scans to one scan.
** if mergePos is NULL then the new scan's robot position will be 
** center of gravity of all scan positions.
*/

struct Scan *Load3DScanFromBuffer(char *buffer,
        struct Position *start, struct Position *end, struct Position *relpos);
/* 
** Converts Jörg's 3D scan buffer representation into a Scan structure.
** start should be the robot position for the first scan point,
** end the robot position for the last scan point.
** relpos specifies the relative scanner position.
** You may specify NULL for relpos for using the
** position defined in the config file.
*/

struct Scan *Load3DScan(FILE *file,
        struct Position *start, struct Position *end, struct Position *relpos);
/* loads file into buffer, then calls Load3DScanFromBuffer */

void FreeScans(struct Scan **scans, long num);
struct Scan **LoadScans(char *filename, long *pnum);
void SaveScans(struct Scan **scans, long num, char *filename);

void SaveMap(struct Scan **scans, long num, char *filename);
/*
** Saves all scans into a single map file by writing out
** the (x,y) positions of all scan points.
*/

struct Scan **AddScan(struct Scan **scans, struct Scan *scan, long *pnum);

void ScansDimensions(struct Scan **scans, long num, 
	double *xoff, double *yoff, double *width, double *height);

#ifdef	__cplusplus
}
#endif
#endif