text:
/*
 BSD 3-Clause License - Please see LICENSE file for full license
 */
#include "mkSVG.h"
#include "tinyxml2/tinyxml2.h"
#include "StyleSheet/StyleSheet/Document.h"
#include <map>
#include <iterator>
#include <regex>
#include <cstring>

#include "mkPath.h"
#include "mkMath.h"
#include "mkPaint.h"
#include "vgCompat.h"
#include <OpenGLES/ES2/gl.h>

#include <OpenGLES/ES2/gl.h>
#include <OpenGLES/ES2/glext.h>
#include "libtess3/Source/tess.h"

#include "SakaSVG.h"

#include <boost/spirit/include/qi_int.hpp>
#include <boost/spirit/include/qi_real.hpp>

namespace MonkSVG {
    MKSVGHandler::path_object_t::~path_object_t() {
        if (fill) delete fill;
        if (stroke) delete stroke;
        if (path) delete path;
    }

    StyleSheet::CssDocument _styleDocument;

	bool SVG::initialize( MKSVGHandler* handler ) {
		_handler = handler;

		return true;
	}

	bool SVG::read( XMLDocument& doc ) {

		XMLElement* root = doc.FirstChildElement( "svg" )->ToElement();
		recursive_parse( root );

        // get bounds information from the svg file, ignoring non-pixel values

        const char* inCString;
        std::string numberWithUnitString;
        std::regex numberWithUnitPattern( "^(-?\\d+)(px)?$" );

        _handler->_minX = 0.0f;
        if ( root->QueryStringAttribute( "x", &inCString ) == XML_SUCCESS ) {
            numberWithUnitString = inCString;
            std::match_results<std::string::const_iterator> matches;
            if ( regex_search( numberWithUnitString, matches, numberWithUnitPattern ) ) {
                _handler->_minX = (int)::strtol( matches[1].str().c_str(), nullptr, 10 );
                SAKA_LOG << "minX=" << _handler->_minX << std::endl;
            }
        }

        _handler->_minY = 0.0f;
        if ( root->QueryStringAttribute( "y", &inCString ) == XML_SUCCESS ) {
            numberWithUnitString = inCString;
            std::match_results<std::string::const_iterator> matches;
            if ( regex_search( numberWithUnitString, matches, numberWithUnitPattern ) ) {
                _handler->_minY = (int)::strtol( matches[1].str().c_str(), nullptr, 10 );
                SAKA_LOG << "minY=" << _handler->_minY << std::endl;
            }
        }

        _handler->_width = 0.0f;
        if ( root->QueryStringAttribute( "width", &inCString ) == XML_SUCCESS ) {
            numberWithUnitString = inCString;
            std::match_results<std::string::const_iterator> matches;
            if ( regex_search( numberWithUnitString, matches, numberWithUnitPattern ) ) {
                _handler->_width = (int)::strtol( matches[1].str().c_str(), nullptr, 10 );
                SAKA_LOG << "width=" << _handler->_width << std::endl;
            }
        }

        _handler->_height = 0.0f;
        if ( root->QueryStringAttribute( "height", &inCString ) == XML_SUCCESS ) {
            numberWithUnitString = inCString;
            std::match_results<std::string::const_iterator> matches;
            if ( regex_search( numberWithUnitString, matches, numberWithUnitPattern ) ) {
                _handler->_height = (int)::strtol( matches[1].str().c_str(), nullptr, 10 );
                SAKA_LOG << "height=" << _handler->_height << std::endl;
            }
        }

        if (_handler->_width == 0.0f && _handler->_height == 0.0f) {
            if ( root->QueryStringAttribute( "viewBox", &inCString) == XML_SUCCESS ) {
                numberWithUnitString = inCString;
                std::match_results<std::string::const_iterator> matches;
                std::regex numberWithUnitPatternVBox( "(\\d+)[ ](\\d+)[ ](\\d+)[ ](\\d+)" );
                if ( regex_search( numberWithUnitString, matches, numberWithUnitPatternVBox ) ) {
                    _handler->_minX = (int)::strtol( matches[1].str().c_str(), nullptr, 10 );
                    _handler->_minY = (int)::strtol( matches[2].str().c_str(), nullptr, 10 );
                    _handler->_width = (int)::strtol( matches[3].str().c_str(), nullptr, 10 );
                    _handler->_height  = (int)::strtol( matches[4].str().c_str(), nullptr, 10 );
                    SAKA_LOG << "minX=" << _handler->_minX << " minY=" << _handler->_minY << " width=" << _handler->_width << " height=" << _handler->_height << std::endl;
                }
            }
        }

		return true;
	}

	void SVG::recursive_parse( XMLElement* element ) {

		if ( element ) {
			for ( XMLElement* sibling = element; sibling != 0; sibling = sibling->NextSiblingElement() ) {
				handle_xml_element( sibling );
			}

            XMLElement* child = element->FirstChildElement();
			// if we don't handle the element recursively go into it
			if( handle_xml_element( child ) == false ) {
				recursive_parse( child );
			}
		}
	}

    static constexpr char tolowerchar(const char c)
    {
        return (c >= 'A' && c <= 'Z') ? c + ('a' - 'A') : c;
    }

    static constexpr uint32_t fnv1a(char const* s, std::size_t count)
    {
        return ((count ? fnv1a(s, count - 1) : 2166136261u) ^ (uint32_t)tolowerchar(s[count])) * 16777619u;
    }

    static constexpr uint32_t operator"" _hash(char const* s, std::size_t count)
    {
        return fnv1a(s, count-1);
    }
    static uint32_t hashStr(const char* s)
    {
        return fnv1a(s, strlen(s)-1);
    }

	bool SVG::handle_xml_element( XMLElement* element ) {
		if( !element )
			return false;

        const auto type = element->Value();
        const auto hash = hashStr(type);

        switch (hash)
        {
            case "style"_hash: return handle_stylesheet(element);
            case "g"_hash: return handle_group( element );
            case "path"_hash: return handle_path( element );
            case "rect"_hash: return handle_rect( element );
            case "line"_hash: return handle_line( element );
            case "polyline"_hash: return handle_polyline( element );
            case "polygon"_hash: return handle_polygon ( element );
            case "symbol"_hash: return handle_symbol( element );
            case "use"_hash: return handle_use( element );
            case "animate"_hash: return handle_animate( element );
            case "set"_hash: return handle_set( element );
            case "animateMotion"_hash: return handle_animateMotion( element );
            case "animateColor"_hash: return handle_animateColor( element );
            case "animateTransform"_hash: return handle_animateTransform( element );
            default:
                SAKA_LOG << "Unknown xml element: " << type << std::endl;
                return false;
        }
	}

	bool SVG::handle_stylesheet( XMLElement* pathElement ) {
        if (pathElement->GetText()) {
            _styleDocument = StyleSheet::CssDocument::parse(pathElement->GetText());
        }
        return false;       // We didn't recurse inside it.
    }

	bool SVG::handle_group( XMLElement* pathElement ) {
        const char* id_;
		if ( pathElement->QueryStringAttribute( "id", &id_) == XML_SUCCESS ) {
			//_handler->onId( id_ );
            SAKA_LOG << "group begin: " << id_ << std::endl;
		}

		_handler->onGroupBegin();

		// handle transform and other parameters
		handle_general_parameter( pathElement );

		// go through all the children
		XMLElement* children = pathElement->FirstChildElement();
        recursive_parse(children);

		_handler->onGroupEnd();

		if ( pathElement->QueryStringAttribute( "id", &id_) == XML_SUCCESS ) {
			//_handler->onId( id_ );
            SAKA_LOG << "group end: " << id_ << std::endl;
		}

        return true;
	}

	bool SVG::handle_path( XMLElement* pathElement ) {

		const char* id_;
		if ( pathElement->QueryStringAttribute( "id", &id_) == XML_SUCCESS ) {
			//_handler->onId( id_ );
            SAKA_LOG << "path: " << id_ << std::endl;
		}

		_handler->onPathBegin();
		const char* d;
		if ( pathElement->QueryStringAttribute( "d", &d ) == XML_SUCCESS ) {
			parse_path_d( d );
		}

		handle_general_parameter( pathElement );

		_handler->onPathEnd();

        return true;
	}

    bool SVG::handle_line( XMLElement* pathElement ) {
        _handler->onPathBegin();

        float pos [4];
        if ( pathElement->QueryFloatAttribute( "x1", &pos[0]) == XML_SUCCESS) {
        }
        if ( pathElement->QueryFloatAttribute( "y1", &pos[1]) == XML_SUCCESS) {
        }
        if ( pathElement->QueryFloatAttribute( "x2", &pos[2]) == XML_SUCCESS) {
        }
        if ( pathElement->QueryFloatAttribute( "y2", &pos[3]) == XML_SUCCESS) {
        }
        _handler->setRelative(false);
        _handler->onPathMoveTo(pos[0], pos[1]);
        _handler->onPathLineTo(pos[2], pos[3]);

        handle_general_parameter( pathElement );

        _handler->onPathEnd();

        return true;
    }

    bool SVG::handle_polyline( XMLElement* pathElement ) {
        _handler->onPathBegin();
        const char* points;
        if ( pathElement->QueryStringAttribute( "points", &points ) == XML_SUCCESS ) {
            parse_polyline_points( points );
        }

        handle_general_parameter( pathElement );

        _handler->onPathEnd();

        return true;
    }

	bool SVG::handle_rect( XMLElement* pathElement ) {
		_handler->onPathBegin();

		float pos[2] = { 0, 0 };
		if ( pathElement->QueryFloatAttribute( "x", &pos[0] ) == XML_SUCCESS ) {
			//parse_path_d( d );
		}
		if ( pathElement->QueryFloatAttribute( "y", &pos[1] ) == XML_SUCCESS ) {
			//parse_path_d( d );
		}
		float sz[2] = { 0, 0 };
		if ( pathElement->QueryFloatAttribute( "width", &sz[0] ) == XML_SUCCESS ) {
			//parse_path_d( d );
		}
		if ( pathElement->QueryFloatAttribute( "height", &sz[1] ) == XML_SUCCESS ) {
			//parse_path_d( d );
		}
		_handler->onPathRect( pos[0], pos[1], sz[0], sz[1] );

		handle_general_parameter( pathElement );

		_handler->onPathEnd();

        return true;
	}

	bool SVG::handle_polygon( XMLElement* pathElement ) {
		_handler->onPathBegin();
		const char* points;
		if ( pathElement->QueryStringAttribute( "points", &points ) == XML_SUCCESS ) {
			parse_points( points );
		}

		handle_general_parameter( pathElement );

		_handler->onPathEnd();

        return true;
	}

    bool SVG::handle_symbol( XMLElement* pathElement )
    {
        const char* id;
        if ( pathElement->QueryStringAttribute( "id", &id ) == XML_SUCCESS ) {
            _symbols[id] = (XMLElement*)pathElement->ShallowClone(nullptr);
        }

        return true;
    }

    bool SVG::handle_use( XMLElement* pathElement )
    {
        const char* href;
        if ( pathElement->QueryStringAttribute( "xlink:href", &href ) == XML_SUCCESS ) {
            std::string id = href+1;	// skip the #
            _handler->onUseBegin();
            // handle transform and other parameters
            handle_general_parameter( pathElement );
            recursive_parse( _symbols[id] );
            _handler->onUseEnd();
        }

        return true;
    }

    bool SVG::handle_animate( XMLElement* pathElement )
    {
        return true;
    }
    bool SVG::handle_set( XMLElement* pathElement )
    {
        return true;
    }
    bool SVG::handle_animateMotion( XMLElement* pathElement )
    {
        return true;
    }
    bool SVG::handle_animateColor( XMLElement* pathElement )
    {
        return true;
    }
    bool SVG::handle_animateTransform( XMLElement* pathElement )
    {
        return true;
    }

    void SVG::handle_animate_parameter( XMLElement* pathElement )
    {

    }

	void SVG::handle_general_parameter( XMLElement* pathElement ) {
		const char* fill;
		if ( pathElement->QueryStringAttribute( "fill", &fill ) == XML_SUCCESS ) {
			_handler->onPathFillColor( string_hex_color_to_uint( fill ) );
		}

		const char* stroke;
		if ( pathElement->QueryStringAttribute( "stroke", &stroke) == XML_SUCCESS ) {
			_handler->onPathStrokeColor( string_hex_color_to_uint( stroke ) );
		}

		const char* stroke_width;
		if ( pathElement->QueryStringAttribute( "stroke-width", &stroke_width) == XML_SUCCESS ) {
			float width = strtof( stroke_width, nullptr );
			_handler->onPathStrokeWidth( width );
		}

		const char* style;
		if ( pathElement->QueryStringAttribute( "style", &style) == XML_SUCCESS ) {
			parse_path_style( style );
		}

        const char* class_;
        if ( pathElement->QueryStringAttribute( "class", &class_) == XML_SUCCESS) {
            parse_path_stylesheet(class_);
        }

		const char* transform;
		if ( pathElement->QueryStringAttribute( "transform", &transform) == XML_SUCCESS ) {
			parse_path_transform( transform );
		}
		const char* id_;
		if ( pathElement->QueryStringAttribute( "id", &id_) == XML_SUCCESS ) {
			_handler->onId( id_ );
		}

		const char* opacity;
		if ( pathElement->QueryStringAttribute( "opacity", &opacity) == XML_SUCCESS ) {
			float o = strtof( opacity, nullptr );
			_handler->onPathFillOpacity( o );
			// TODO: ??? stroke opacity???
		}
		if ( pathElement->QueryStringAttribute( "fill-opacity", &opacity) == XML_SUCCESS ) {
			float o = strtof( opacity, nullptr );
			_handler->onPathFillOpacity( o );
		}

		const char* fillrule;
		if ( pathElement->QueryStringAttribute( "fill-rule", &fillrule) == XML_SUCCESS ) {
			_handler->onPathFillRule( fillrule );
		}

	}

	float SVG::d_string_to_float( char *c, char *max, char** str ) {
		while ( isspace(*c) ) {
			c++;
			(*str)++;
		}
		while ( *c == ',' ) {
			c++;
			(*str)++;
		}

        using boost::spirit::qi::parse;
        using boost::spirit::qi::float_;

        float result;
        parse(c, max, float_, result);
        if (str) *str = c;
		return (float)result;
	}

	int SVG::d_string_to_int( char *c, char *max, char **str ) {
		while ( isspace(*c) ) {
			c++;
			(*str)++;
		}
		while ( *c == ',' ) {
			c++;
			(*str)++;
		}

        using boost::spirit::qi::parse;
        using boost::spirit::qi::int_;

        int result;
        parse(c, max, int_, result);
        if (str) *str = c;
        return result;
	}

    uint32_t SVG::string_hex_color_to_uint( const std::string& hexstring ) {
		uint32_t color = (uint32_t)strtol( hexstring.c_str() + 1, 0, 16 );

		if ( hexstring.length() == 7 ) {	// fix up to rgba if the color is only rgb
			color = color << 8;
			color |= 0x000000ff;
		}

		return color;
	}

	bool SVG::nextState( char** c, char* state ) {
		if ( **c == '\0') {
			return false;
		}

		while ( isspace(**c) ) {
			if ( **c == '\0') {
				return false;
			}
			(*c)++;
		}
		if ( isalpha( **c ) ) {
			*state = **c;
			(*c)++;
//			if ( **c == '\0') {
//				*state = '\0';
//				return;
//			}

			if ( islower(*state) ) {	// if lower case then relative coords (see SVG spec)
				_handler->setRelative( true );
			} else {
				_handler->setRelative( false );
			}
		}
        SAKA_LOG << "state: " << *state << std::endl;
        return true;
	}

	void SVG::parse_path_transform( const std::string& tr )	{
		size_t p = tr.find( "translate" );
		if ( p != std::string::npos ) {
			size_t left = tr.find( "(" );
			size_t right = tr.find( ")" );
			std::string values = tr.substr( left+1, right-1 );
			char* c = const_cast<char*>( values.c_str() );
            char* max = c + (right - left - 1);
			float x = d_string_to_float( c, max, &c );
			float y = d_string_to_float( c, max, &c );
			_handler->onTransformTranslate( x, y );
            SAKA_LOG << "Translate " << x << ", " << y << std::endl;
		} else if ( tr.find( "rotate" ) != std::string::npos ) {
			size_t left = tr.find( "(" );
			size_t right = tr.find( ")" );
			std::string values = tr.substr( left+1, right-1 );
			char* c = const_cast<char*>( values.c_str() );
            char* max = c + (right - left - 1);
			float a = d_string_to_float( c, max, &c );
			_handler->onTransformRotate( a );	// ??? radians or degrees ??
            SAKA_LOG << "Rotate " << a << std::endl;
		} else if ( tr.find( "matrix" ) != std::string::npos ) {
			size_t left = tr.find( "(" );
			size_t right = tr.find( ")" );
			std::string values = tr.substr( left+1, right-1 );
			char* cc = const_cast<char*>( values.c_str() );
            char* max = cc + (right - left - 1);
			float a = d_string_to_float( cc, max, &cc );
			float b = d_string_to_float( cc, max, &cc );
			float c = d_string_to_float( cc, max, &cc );
			float d = d_string_to_float( cc, max, &cc );
			float e = d_string_to_float( cc, max, &cc );
			float f = d_string_to_float( cc, max, &cc );
			_handler->onTransformMatrix( a, b, c, d, e, f );
            SAKA_LOG << "Matrix " << a << ", " << b << ", " << c << ", " << d << ", " << e << ", " << f << std::endl;
		}
	}

	void SVG::parse_path_d( const std::string& d ) {
		char* c = const_cast<char*>( d.c_str() );
        char* max = c + d.length();
		char state = *c;
		while ( nextState( &c, &state ) ) {

			switch ( state ) {
				case 'm':
				case 'M':
				{
					float x = d_string_to_float( c, max, &c );
					float y = d_string_to_float( c, max, &c );
                    SAKA_LOG << "  move to " << state << ": " << x << ", " << y << std::endl;
					_handler->onPathMoveTo( x, y );
				}
				break;

				case 'l':
				case 'L':
				{
					float x = d_string_to_float( c, max, &c );
					float y = d_string_to_float( c, max, &c );
                    SAKA_LOG << "  line to " << state << ": " << x << ", " << y << std::endl;
					_handler->onPathLineTo( x, y );

				}
				break;

				case 'h':
				case 'H':
				{
					float x = d_string_to_float( c, max, &c );
                    SAKA_LOG << "  H line " << state << ": " << x << std::endl;
					_handler->onPathHorizontalLine( x );
				}
				break;

				case 'v':
				case 'V':
				{
                    float y = d_string_to_float( c, max, &c );
                    SAKA_LOG << "  V line " << state << ": " << y << std::endl;
					_handler->onPathVerticalLine( y );

				}
				break;

				case 'c':
				case 'C':
				{
					float x1 = d_string_to_float( c, max, &c );
					float y1 = d_string_to_float( c, max, &c );
					float x2 = d_string_to_float( c, max, &c );
					float y2 = d_string_to_float( c, max, &c );
					float x3 = d_string_to_float( c, max, &c );
					float y3 = d_string_to_float( c, max, &c );
                    SAKA_LOG << "  cubic " << state << ": " << x1 << ", " << y1 << ", " << x2 << ", " << y2 << ", " << x3 << ", " << y3 << std::endl;
					_handler->onPathCubic(x1, y1, x2, y2, x3, y3);
				}
				break;

				case 's':
				case 'S':
				{
					float x2 = d_string_to_float( c, max, &c );
					float y2 = d_string_to_float( c, max, &c );
					float x3 = d_string_to_float( c, max, &c );
					float y3 = d_string_to_float( c, max, &c );
                    SAKA_LOG << "  Scubic " << state << ": " << x2 << ", " << y2 << ", " << x3 << ", " << y3 << std::endl;
					_handler->onPathSCubic(x2, y2, x3, y3);
                }
				break;

				case 'a':
				case 'A':
				{
					float rx = d_string_to_float( c, max, &c );
					float ry = d_string_to_float( c, max, &c );
					float x_axis_rotation = d_string_to_float( c, max, &c );
					int large_arc_flag = d_string_to_int( c, max, &c );
					int sweep_flag = d_string_to_int( c, max, &c );
					float x = d_string_to_float( c, max, &c );
					float y = d_string_to_float( c, max, &c );
                    SAKA_LOG << "  arc " << state << ": r " << rx << ", " << ry << ", rot " << x_axis_rotation << ", large " << large_arc_flag << ", sweep " << sweep_flag << ", pos " << x << ", " << y << std::endl;
					_handler->onPathArc( rx, ry, x_axis_rotation, large_arc_flag, sweep_flag, x, y );

				}
				break;

				case 'z':
				case 'Z':
				{
                    SAKA_LOG << "  )" << std::endl;
					_handler->onPathClose();
                }
				break;

                case 'q':
                case 'Q':
                {
                    float x1 = d_string_to_float(c, max, &c);
                    float y1 = d_string_to_float(c, max, &c);
                    float x2 = d_string_to_float(c, max, &c);
                    float y2 = d_string_to_float(c, max, &c);
                    SAKA_LOG << "  quad " << state << ": " << x1 << ", " << y1 << ", " << x2 << ", " << y2 << std::endl;
                    _handler->onPathQuad(x1, y1, x2, y2);
                }
                break;

				default:
                    SAKA_LOG << "  unknown state " << state << std::endl;
					c++;
					break;
			}
		}
	}

    void SVG::parse_path_stylesheet( std::string ps ) {

        if (_styleDocument.getElementCount()) {

            StyleSheet::CssElement theElement = _styleDocument.getElement(StyleSheet::CssSelector::CssClassSelector(ps));

            if (theElement.getPropertyCount()) {

                StyleSheet::CssProperty property = theElement.getProperties().getProperty("fill");
                std::string value = property.getValue();

                if ( !value.empty() ) {
                    if( value != "none" && !value.empty())
                        _handler->onPathFillColor( string_hex_color_to_uint( value ) );
                }

                property = theElement.getProperties().getProperty("fill-rule");
                value = property.getValue();
                if ( !value.empty() ) {
                    _handler->onPathFillRule( value );
                }

                property = theElement.getProperties().getProperty("fill-opacity");
                value = property.getValue();
                if ( !value.empty() ) {
                    float o = strtof( value.c_str(), nullptr );
                    _handler->onPathFillOpacity( o );
                }

                property = theElement.getProperties().getProperty("stroke");
                value = property.getValue();

                if ( !value.empty() ) {
                    if( value != "none" && !value.empty())
                        _handler->onPathStrokeColor( string_hex_color_to_uint( value ) );
                }

                property = theElement.getProperties().getProperty("stroke-width");
                value = property.getValue();
                if ( !value.empty() ) {
                    float width = strtof( value.c_str(), nullptr );
                    _handler->onPathStrokeWidth( width );
                }

/*                property = theElement.getProperties().getProperty( "stroke-linecap" );
                value = property.getValue();
                if ( value != "none" && !value.empty() ) {
                    _handler->onPathStrokeCapStyle( value );
                }

                property = theElement.getProperties().getProperty( "stroke-linejoin" );
                value = property.getValue();
                if ( value != "none" && !value.empty() ) {
                    _handler->onPathStrokeLineJoin( value );
                }

                property = theElement.getProperties().getProperty( "stroke-dasharray" );
                value = property.getValue();
                if ( value != "none" && !value.empty() ) {
                    vector<int> dasharray;
                    StyleSheet::tokenizer(value, ", \t", [&dasharray](const string& token)
                    {
                        dasharray.push_back(atoi(token.c_str()));
                    });
                    _handler->onPathStrokDashPattern((int *)dasharray.data());
                }

                property = theElement.getProperties().getProperty( "stroke-miterlimit" );
                value = property.getValue();
                if ( !value.empty()) {
                    float o = atof( value.c_str() );
                    _handler->onPathStrokeMiterLimit( o );
                }
*/
                property = theElement.getProperties().getProperty("stroke-opacity");
                value = property.getValue();
                if ( !value.empty() ) {
                    float o = strtof( value.c_str(), nullptr );
                    _handler->onPathStrokeOpacity( o );
                }

                property = theElement.getProperties().getProperty("opacity");
                value = property.getValue();
                if ( !value.empty() ) {
                    float o = strtof( value.c_str(), nullptr );
                    _handler->onPathFillOpacity( o );
                }

            }

        }
    }

	// semicolon-separated property declarations of the form "name : value" within the ‘style’ attribute
	void SVG::parse_path_style( const std::string& ps ) {
        Saka::map< std::string, std::string > style_key_values;

        char *initial_str = new char[ps.length() + 1];
        char *str = initial_str;
        strcpy(str, ps.c_str());
        const char* const style_separator = ";";
        const char keyval_separator = ':';
        char *values = strtok(str, style_separator);

        while (values != NULL) {
            char *value = strchr(values, keyval_separator);
            char *key = values;
            if (value != NULL) {
                *value = '\0';
                ++value;
                style_key_values[std::string(key)] = std::string(value);
            }

            values = strtok(NULL, style_separator);
        }

        delete [] initial_str;

        Saka::map<std::string, std::string>::iterator kv = style_key_values.find( std::string("fill") );
		if ( kv != style_key_values.end() ) {
			if( kv->second != "none" )
				_handler->onPathFillColor( string_hex_color_to_uint( kv->second ) );
		}

        kv = style_key_values.find( "fill-rule" );
        if ( kv != style_key_values.end() ) {
            _handler->onPathFillRule( kv->second );
        }

        kv = style_key_values.find( "fill-opacity" );
        if ( kv != style_key_values.end() ) {
            float o = strtof( kv->second.c_str(), nullptr );
            _handler->onPathFillOpacity( o );
        }

		kv = style_key_values.find( "stroke" );
		if ( kv != style_key_values.end() ) {
			if( kv->second != "none" )
				_handler->onPathStrokeColor( string_hex_color_to_uint( kv->second ) );
		}

		kv = style_key_values.find( "stroke-width" );
		if ( kv != style_key_values.end() ) {
			float width = strtof( kv->second.c_str(), nullptr );
			_handler->onPathStrokeWidth( width );
		}

/*        kv = style_key_values.find( "stroke-linejoin" );
        if ( kv != style_key_values.end() ) {
            if ( kv->second != "none" ) {
                _handler->onPathStrokeLineJoin( kv->second );
            }
        }

        kv = style_key_values.find( "stroke-linecap" );
        if ( kv != style_key_values.end() ) {
            if ( kv->second != "none" ) {
                _handler->onPathStrokeCapStyle( kv->second );
            }
        }

        kv = style_key_values.find( "stroke-dasharray" );
        if ( kv != style_key_values.end() ) {
            if ( kv->second != "none" ) {
                vector<int> dasharray;
                StyleSheet::tokenizer(kv->second, ", \t", [&dasharray](const string& token)
                                      {
                                          dasharray.push_back(atoi(token.c_str()));
                                      });
                _handler->onPathStrokDashPattern((int *)dasharray.data());
            }
        }

        kv = style_key_values.find( "stroke-miterlimit" );
        if ( kv != style_key_values.end() ) {
            float o = atof( kv->second.c_str() );
            _handler->onPathStrokeMiterLimit( o );
        }
*/
        kv = style_key_values.find( "stroke-opacity" );
        if ( kv != style_key_values.end() ) {
            float o = strtof( kv->second.c_str(), nullptr );
            _handler->onPathStrokeOpacity( o );
        }

		kv = style_key_values.find( "opacity" );
		if ( kv != style_key_values.end() ) {
			float o = strtof( kv->second.c_str(), nullptr );
			_handler->onPathFillOpacity( o );
			// ?? TODO: stroke Opacity???
		}
    }

    void SVG::parse_polyline_points( const std::string& points ) {
        float xy[2];
        int xy_offset = 0;  // 0:x, 1:y
        bool first = true;
        _handler->setRelative(false);
        StyleSheet::tokenizer(points, ", \t", [&xy, &xy_offset, &first, this](const std::string& token)
                              {
                                  xy[xy_offset++] = (float)atof(token.c_str());

                                  if (xy_offset == 2) {
                                      xy_offset = 0;
                                      if (first) {
                                          _handler->onPathMoveTo( xy[0], xy[1] );
                                          first = false;
                                      } else
                                          _handler->onPathLineTo( xy[0], xy[1] );
                                  }
                              });
        //_handler->onPathClose();
    }

    void SVG::parse_points( const std::string& points ) {
        float xy[2];
        int xy_offset = 0;  // 0:x, 1:y
        bool first = true;
        _handler->setRelative(false);
        char *str = new char[points.length() + 1];
        strcpy(str, points.c_str());
        char *p = strtok(str, ", \t");
        while (p != NULL) {
            xy[xy_offset++] = (float)atof(p);

            if (xy_offset == 2) {
                xy_offset = 0;
                if (first) {
                    _handler->onPathMoveTo( xy[0], xy[1] );
                    first = false;
                } else
                    _handler->onPathLineTo( xy[0], xy[1] );
            }

            p = strtok(NULL, ", \t");
        }

        delete [] str;
        _handler->onPathClose();
	}

    MKSVGHandler::MKSVGHandler()
    :    _minX( INT_MAX )
    ,    _minY( INT_MAX )
    ,    _width( INT_MIN )
    ,    _height( INT_MIN )
    ,	_mode( kGroupParseMode )
    ,	_current_group( &_root_group )
    ,	_blackBackFill( 0 )
    ,	_use_opacity( 1 )
    ,   _has_transparent_colors( false )

    ,	_stroke_line_width( 1.0f )
    ,	_stroke_paint( 0 )
    ,	_fill_paint( 0 )
    ,	_fill_rule( VG_EVEN_ODD )
    ,	_tessellationIterations( 4 )
    ,   _use_transform( Matrix33(1.0) )
    ,   _active_matrix( Matrix33(1.0) )
    ,   _root_transform( Matrix33(1.0) )

    {
        _blackBackFill = createPaint();
        float fcolor[4] = { 0,0,0,1 };
        _blackBackFill->setPaintColor(fcolor);

        //_root_transform.setScale( 1, -1 );

        _vertices.reserve(1000000/sizeof(vertexData_t));
        _sentinelPoints.reserve(200);
        _ebo.reserve(1000000/sizeof(ebo_t));
    }

    MKSVGHandler::~MKSVGHandler() {
        delete _blackBackFill;
    }

    void MKSVGHandler::draw_recursive( group_t& group ) {

        // push the group matrix onto the stack
        pushTransform( group.transform ); setTransform( topTransform() );

        for ( Saka::list<path_object_t>::iterator it = group.path_objects.begin(); it != group.path_objects.end(); it++ ) {
            path_object_t& po = *it;
            uint32_t draw_params = 0;
            if ( po.fill ) {
                setFillPaint(po.fill);
                draw_params |= VG_FILL_PATH;
            }

            if ( po.stroke ) {
                setStrokePaint( po.stroke );
                setStrokeLineWidth(po.stroke_width);
                draw_params |= VG_STROKE_PATH;
            }

            if( draw_params == 0 ) {	// if no stroke or fill use the default black fill
                setFillPaint( _blackBackFill );
                draw_params |= VG_FILL_PATH;
            }

            // set the fill rule
            setFillRule(po.fill_rule);
            // trasnform
            pushTransform( po.transform );	setTransform( topTransform() );
            po.path->draw( draw_params );
            popTransform();	setTransform( topTransform() );
        }

        for ( Saka::list<group_t>::iterator it = group.children.begin(); it != group.children.end(); it++ ) {
            draw_recursive( *it );
        }

        popTransform();	setTransform( topTransform() );
    }

    void MKSVGHandler::optimize() {
        // clear out the transform stack
        _transform_stack.clear();

        Matrix33 m = getTransform();
        Matrix33 top = m * rootTransform();
        pushTransform( top );

        // SVG is origin at the top, left (openvg is origin at the bottom, left)
        // so need to flip
        //		Matrix33 flip;
        //		flip.setScale( 1, -1 );
        //		pushTransform( flip );

        draw_recursive( _root_group );

        setTransform( m );	// restore matrix
        _transform_stack.clear();
    }

    void MKSVGHandler::onGroupBegin() {
        _mode = kGroupParseMode;
        _current_group->children.push_back( group_t() );
        group_t* top = &_current_group->children.back();
        top->parent = _current_group;
        _current_group = top;
        // copy any use transform
        _current_group->transform = _use_transform;
    }
    void MKSVGHandler::onGroupEnd() {
        _current_group = _current_group->parent;
    }

    void MKSVGHandler::onPathBegin() {
        _mode = kPathParseMode;
        _current_group->current_path = new path_object_t();
        _current_group->current_path->path = createPath();
        // inherit group settings
        _current_group->current_path->fill			= _current_group->fill;
        _current_group->current_path->stroke		= _current_group->stroke;
        _current_group->current_path->stroke_width	= _current_group->stroke_width;
        _current_group->current_path->fill_rule		= _current_group->fill_rule;

    }

    void MKSVGHandler::onPathEnd() {

        // onPathClose()

        _current_group->path_objects.push_back( *_current_group->current_path );
        // Do not delete what was copy-constructed, only delete the object itself
        _current_group->current_path->path = nullptr;
        _current_group->current_path->fill = nullptr;
        _current_group->current_path->stroke = nullptr;
        delete _current_group->current_path;
        _current_group->current_path = nullptr;
    }

    void MKSVGHandler::onPathMoveTo( float x, float y ) {
        unsigned char seg = VG_MOVE_TO | openVGRelative();
        float data[2];

        data[0] = x; data[1] = y;
        _current_group->current_path->path->appendData( 1, &seg, data );

    }

    void MKSVGHandler::onPathClose(){
        unsigned char seg = VG_CLOSE_PATH;
        float data = 0.0f;
        _current_group->current_path->path->appendData( 1, &seg, &data );

    }
    void MKSVGHandler::onPathLineTo( float x, float y ) {
        unsigned char seg = VG_LINE_TO | openVGRelative();
        float data[2];

        data[0] = x; data[1] = y;
        _current_group->current_path->path->appendData( 1, &seg, data );

    }

    void MKSVGHandler::onPathHorizontalLine( float x ) {
        unsigned char seg = VG_HLINE_TO | openVGRelative();
        float data[1];
        data[0] = x;
        _current_group->current_path->path->appendData( 1, &seg, data );

    }
    void MKSVGHandler::onPathVerticalLine( float y ) {
        unsigned char seg = VG_VLINE_TO | openVGRelative();
        float data[1];
        data[0] = y;
        _current_group->current_path->path->appendData( 1, &seg, data );

    }

    void MKSVGHandler::onPathCubic( float x1, float y1, float x2, float y2, float x3, float y3 ) {
        unsigned char seg = VG_CUBIC_TO | openVGRelative();
        float data[6];

        data[0] = x1; data[1] = y1;
        data[2] = x2; data[3] = y2;
        data[4] = x3; data[5] = y3;
        _current_group->current_path->path->appendData( 1, &seg, data);

    }

    void MKSVGHandler::onPathSCubic( float x2, float y2, float x3, float y3 ) {
        unsigned char seg = VG_SCUBIC_TO | openVGRelative();
        float data[4];

        data[0] = x2; data[1] = y2;
        data[2] = x3; data[3] = y3;
        _current_group->current_path->path->appendData( 1, &seg, data);

    }

    void MKSVGHandler::onPathQuad( float x1, float y1, float x2, float y2) {
        unsigned char seg = VG_QUAD_TO | openVGRelative();
        float data[4];
        data[0] = x1; data[1] = y1;
        data[2] = x2; data[3] = y2;
        _current_group->current_path->path->appendData( 1, &seg, data);
    }

    void MKSVGHandler::onPathArc( float rx, float ry, float x_axis_rotation, int large_arc_flag, int sweep_flag, float x, float y ) {

        unsigned char seg = openVGRelative();
        if ( large_arc_flag ) {
            if (sweep_flag) {
                seg |= VG_LCCWARC_TO;
            } else {
                seg |= VG_LCWARC_TO;
            }
        } else {
            if (sweep_flag) {
                seg |= VG_SCCWARC_TO;
            } else {
                seg |= VG_SCWARC_TO;
            }

        }
        float data[5];

        data[0] = rx;
        data[1] = ry;
        data[2] = x_axis_rotation;
        data[3] = x;
        data[4] = y;

        _current_group->current_path->path->appendData( 1, &seg, data);
    }

    void MKSVGHandler::onPathRect( float x, float y, float w, float h ) {
        static const unsigned char segments[5] = {
            VG_MOVE_TO | VG_ABSOLUTE,
            VG_HLINE_TO | VG_ABSOLUTE,
            VG_VLINE_TO | VG_ABSOLUTE,
            VG_HLINE_TO | VG_ABSOLUTE,
            VG_CLOSE_PATH};
        const float data[5] = {x, y, x + w, y + h, x};
        _current_group->current_path->path->appendData(5, segments, data);
    }

    void MKSVGHandler::onPathFillColor( unsigned int color ) {
        if( _mode == kGroupParseMode ) {
            _current_group->fill = createPaint();
            float fcolor[4] = { float( (color & 0xff000000) >> 24)/255.0f,
                float( (color & 0x00ff0000) >> 16)/255.0f,
                float( (color & 0x0000ff00) >> 8)/255.0f,
                _use_opacity };
            _current_group->fill->setPaintColor(fcolor);

        } else {
            _current_group->current_path->fill = createPaint();
            float fcolor[4] = { float( (color & 0xff000000) >> 24)/255.0f,
                float( (color & 0x00ff0000) >> 16)/255.0f,
                float( (color & 0x0000ff00) >> 8)/255.0f,
                _use_opacity  };
            _current_group->current_path->fill->setPaintColor(fcolor);
        }
    }

    void MKSVGHandler::onPathFillOpacity( float o ) {
        static const float fcolorBlack[4] = { 0,0,0,1 };
        float fcolor[4];
        if( _mode == kGroupParseMode ) {
            if( _current_group->fill == 0 ) {	// if no fill create a black fill
                _current_group->fill = createPaint();
                _current_group->fill->setPaintColor(fcolorBlack);
            }
            _current_group->fill->getPaintColor( &fcolor[0] );
            // set the opacity
            fcolor[3] = o;
            _current_group->fill->setPaintColor(fcolor);

        } else if( _mode == kPathParseMode ) {
            if( _current_group->current_path->fill == 0 ) {	// if no fill create a black fill
                _current_group->current_path->fill = createPaint();
                _current_group->current_path->fill->setPaintColor(fcolorBlack);
            }

            _current_group->current_path->fill->getPaintColor( &fcolor[0] );
            // set the opacity
            fcolor[3] = o;
            _current_group->current_path->fill->setPaintColor(fcolor);
        } else if( _mode == kUseParseMode ) {
            _use_opacity = o;
            if( _current_group->fill == 0 ) {	// if no fill create a black fill
                _current_group->fill = createPaint();
                _current_group->fill->setPaintColor(fcolorBlack);
            }
            _current_group->fill->getPaintColor( &fcolor[0] );
            // set the opacity
            fcolor[3] = o;
            _current_group->fill->setPaintColor(fcolor);
            _use_opacity = o;

        }
        _has_transparent_colors = _has_transparent_colors || (o < 1.0f);
    }
    void MKSVGHandler::onPathStrokeColor( unsigned int color ) {
        if( _mode == kGroupParseMode ) {
            _current_group->stroke = createPaint();
            float fcolor[4] = { float( (color & 0xff000000) >> 24)/255.0f,
                float( (color & 0x00ff0000) >> 16)/255.0f,
                float( (color & 0x0000ff00) >> 8)/255.0f,
                _use_opacity };
            _current_group->stroke->setPaintColor(fcolor);
        } else {
            _current_group->current_path->stroke = createPaint();
            float fcolor[4] = { float( (color & 0xff000000) >> 24)/255.0f,
                float( (color & 0x00ff0000) >> 16)/255.0f,
                float( (color & 0x0000ff00) >> 8)/255.0f,
                _use_opacity };
            _current_group->current_path->stroke->setPaintColor(fcolor);
        }
    }
    void MKSVGHandler::onPathStrokeOpacity( float o ) {
        float fcolor[4];
        if( _mode == kGroupParseMode ) {
            _current_group->stroke->getPaintColor( &fcolor[0] );
            // set the opacity
            fcolor[3] = o;
            _current_group->stroke->setPaintColor(fcolor);

        } else {
            _current_group->current_path->stroke->getPaintColor( &fcolor[0] );
            // set the opacity
            fcolor[3] = o;
            _current_group->current_path->stroke->setPaintColor(fcolor);
        }
        _has_transparent_colors = _has_transparent_colors || (o < 1.0f);
    }

    void MKSVGHandler::onPathStrokeWidth( float width ) {
        if( _mode == kGroupParseMode ) {
            _current_group->stroke_width = width;
        } else {
            _current_group->current_path->stroke_width = width;
        }
    }

    void MKSVGHandler::onPathFillRule( const std::string& rule ) {
        if( _mode == kGroupParseMode ) {
            if( rule == "nonzero" ) {
                _current_group->fill_rule = VG_NON_ZERO;
            } else if( rule == "evenodd" ) {
                _current_group->fill_rule = VG_EVEN_ODD;
            }

        } else {
            if( rule == "nonzero" ) {
                _current_group->current_path->fill_rule = VG_NON_ZERO;
            } else if( rule == "evenodd" ) {
                _current_group->current_path->fill_rule = VG_EVEN_ODD;
            }
        }
    }

    void MKSVGHandler::onTransformTranslate( float x, float y ) {
        if( _mode == kGroupParseMode ) {
            MonkVG::translate(_current_group->transform, x, y );
        } else if( kPathParseMode ) {	//

            MonkVG::translate(_current_group->current_path->transform, x, y );
        }
    }
    void MKSVGHandler::onTransformScale( float s ) {
        if( _mode == kGroupParseMode ) {
            MonkVG::scale(_current_group->transform, s, s );
        } else if( _mode == kPathParseMode ) { // kPathParseMode
            MonkVG::scale(_current_group->current_path->transform, s, s );
        } else if( _mode == kUseParseMode ) {
            MonkVG::scale(_use_transform, s, s);
        }
    }
    void MKSVGHandler::onTransformRotate( float r ) {
        if( _mode == kGroupParseMode ) {
            MonkVG::rotate(_current_group->transform, r );
        } else if( _mode == kPathParseMode ) {
            MonkVG::rotate(_current_group->current_path->transform, r );
        } else if( _mode == kUseParseMode ) {
            MonkVG::rotate(_use_transform, r );
        }
    }
    void MKSVGHandler::onTransformMatrix( float a, float b, float c, float d, float e, float f ) {
        Matrix33 t(a, b, c, d, e, f, 0.f, 0.f, 1.f);
        if( _mode == kGroupParseMode ) {
            _current_group->transform = t;
        } else if( _mode == kPathParseMode ) { // kPathParseMode
            _current_group->current_path->transform = t;
        } else if( _mode == kUseParseMode ) {
            _use_transform = t;//topTransform();
        }
    }

    void MKSVGHandler::onId( const std::string& id_ ) {
        if( _mode == kGroupParseMode ) {
            _current_group->id  = id_;
        } else if( _current_group->current_path ) { // kPathParseMode
            _current_group->current_path->id = id_;
        }
    }

    void MKSVGHandler::onUseBegin() {
        _mode = kUseParseMode;
        _use_opacity = 1.0;
    }
    void MKSVGHandler::onUseEnd() {
        _use_transform = Matrix33(1.0);
        _use_opacity = 1.0;
    }
}

namespace MonkSVG {
    /// factories

    MKPath* MKSVGHandler::createPath( ) {

        MKPath *path = new MKPath(this);
        assert( path );
        return (MKPath*)path;
    }

    MKPaint* MKSVGHandler::createPaint() {
        MKPaint *paint = new MKPaint();
        assert( paint );
        return (MKPaint*)paint;
    }

    /// state
    void MKSVGHandler::setStrokePaint( MKPaint* paint ) {
        if ( paint != _stroke_paint ) {
            _stroke_paint = paint;
        }
    }

    void MKSVGHandler::setFillPaint( MKPaint* paint ) {
        if ( paint != _fill_paint ) {
            _fill_paint = paint;
        }

    }

    void MKSVGHandler::setIdentity() {
        _active_matrix = Matrix33(1.0);
    }

    const Matrix33& MKSVGHandler::getTransform( ) const {
        return _active_matrix;
    }

    void MKSVGHandler::setTransform( const Matrix33& t )  {
        _active_matrix = t;
    }

    void MKSVGHandler::multiply( const Matrix33& t ) {
        _active_matrix *= t;
    }

    void MKSVGHandler::scale( float sx, float sy ) {
        _active_matrix = glm::scale(_active_matrix, v2_t(sx, sy));
    }
    void MKSVGHandler::translate( float x, float y ) {
        _active_matrix = glm::translate(_active_matrix, v2_t(x, y));
    }
    float MKSVGHandler::angle()
    {
        return std::acosf((_active_matrix)[0][0]);
    }

    void MKSVGHandler::rotate( float angle ) {
        _active_matrix = glm::rotate(_active_matrix, angle);
    }

    void MKSVGHandler::rotate(float angle, float x, float y, float z) {

        translate(x, y);
        rotate(angle);

    }

    // Based on https://hal.archives-ouvertes.fr/inria-00072100/document
    static inline int32_t orientation(const int32_t a[2], const int32_t b[2], const int32_t c[2])
    {
        int32_t result = (a[0]-c[0]) * (b[1]-c[1]) - (a[1]-c[1]) * (b[0]-c[0]);
        return result;
    }

    void MKSVGHandler::addPathVertexData( GLfloat* strokeVerts, size_t strokeVertCnt, GLbitfield paintModes ) {
        /*
        int32_t v[6];
        if ( paintModes & VG_STROKE_PATH) {
            // get the paint color
            const float* fc = getStrokePaint()->getPaintColor();

            const MonkVG::Color color(
                                      GLuint(fc[0] * 255.0f),
                                      GLuint(fc[1] * 255.0f),
                                      GLuint(fc[2] * 255.0f),
                                      GLuint(fc[3] * 255.0f));

            // get vertices and transform them
            int32_t* firstV = &v[0];    // Don't use a,b,c as these need to keep in order
            int32_t* secondV = &v[2];
            int32_t* thirdV = &v[4];

            int vertcnt = 0;
            for ( unsigned long i = 0; i < strokeVertCnt * 2; i+=2, vertcnt++ ) {
                v2_t affine = affineTransform(_active_matrix, &strokeVerts[i]) * precisionMult;

                // for stroke we need to convert from a strip to triangle
                switch ( vertcnt ) {
                    case 0:
                        firstV[0] = static_cast<int32_t>(affine[0]);
                        firstV[1] = static_cast<int32_t>(affine[1]);
                        break;
                    case 1:
                        secondV[0] = static_cast<int32_t>(affine[0]);
                        secondV[1] = static_cast<int32_t>(affine[1]);
                        break;
                    default:
                    {
                        thirdV[0] = static_cast<int32_t>(affine[0]);
                        thirdV[1] = static_cast<int32_t>(affine[1]);

                        // Next will override what was in firstV.
                        std::swap(firstV, thirdV);
                        std::swap(firstV, secondV);

                        addTriangle(v, color);
                        break;
                    }
                }
            }
            finalizeTriangleBatch();
        } */
    }

    void MKSVGHandler::finalize(Saka::SVG* dest) {
        optimize();
        // Move triangles to vertexes
        printf("numVbo = %d, numEbo = %d\n", (int)_vertices.size(), (int)_ebo.size());

        Saka::SharedVAO sVao = dest->vao = Saka::SharedVAO::make_shared();
        Saka::SharedVBO sVbo = Saka::SharedVBO::make_shared();
        Saka::SharedEBO sEbo = dest->ebo = Saka::SharedEBO::make_shared();
        Saka::GLMgr::instance().bind(sVao).bind(sVbo).bind(sEbo);
        sVbo->bufferData((GLsizeiptr)(_vertices.size() * sizeof(vertexData_t)), &_vertices[0], GL_STATIC_DRAW);
        sVbo->setNumAttribs(2);
        sVbo->addAttrib(2, GL_FLOAT, GL_TRUE, sizeof(vertexData_t), (GLvoid*)vertexData_t::k_position);
        sVbo->addAttrib(4, GL_UNSIGNED_BYTE, GL_TRUE, sizeof(vertexData_t), (GLvoid*)vertexData_t::k_color);
        sVao->addVbo(sVbo);
        sEbo->bufferData((GLsizeiptr)(_ebo.size() * sizeof(GLuint)), &_ebo[0], GL_STATIC_DRAW, GL_TRIANGLES, (GLsizei)(_ebo.size()), GL_UNSIGNED_INT);
    }
}

max_stars_repo_name: adozenlines/MonkSVG

max_issues_repo_name: sakamura/SakaSVG

max_forks_repo_name: sakamura/SakaSVG


text:
// MIT License
//
// Copyright (c) 2018 Michal Siedlaczek
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

//! \file
//! \author     Michal Siedlaczek
//! \copyright  MIT License

#pragma once

#include <iostream>
#include <string>

#include <boost/te.hpp>
#include <fmt/format.h>
#include <gsl/span>
#include <irkit/algorithm/query.hpp>
#include <irkit/parsing/stemmer.hpp>
#include <irkit/score.hpp>

namespace irk {

struct Empty_Tag {
} empty_tag;

enum class Traversal_Type { TAAT, DAAT };
inline std::ostream& operator<<(std::ostream& os, Traversal_Type type)
{
    switch (type) {
    case Traversal_Type::TAAT: os << "taat"; break;
    case Traversal_Type::DAAT: os << "daat"; break;
    default: throw std::domain_error("Traversal_Type: non-exhaustive switch");
    }
    return os;
}

struct Daat_Traveral_Tag {
} daat_traversal;

inline std::ostream& operator<<(std::ostream& os, Daat_Traveral_Tag)
{
    os << "daat";
    return os;
}

struct Taat_Traveral_Tag {
} taat_traversal;

inline std::ostream& operator<<(std::ostream& os, Taat_Traveral_Tag)
{
    os << "taat";
    return os;
}

template<class Score_Tag, class Index>
auto fetch_scorers(Index const& index, gsl::span<std::string const>& terms, Score_Tag score_tag)
{
    std::vector<decltype(index.term_scorer(0, score_tag))> scorers;
    for (const auto& term : terms) {
        if (auto term_id = index.term_id(term); term_id) {
            scorers.push_back(index.term_scorer(term_id.value(), score_tag));
        } else {
            scorers.push_back(index.term_scorer(0, score_tag));
        }
    }
    return scorers;
}

template<typename Index>
inline auto fetched_query_postings(Index const& index, gsl::span<std::string const>& query_terms)
{
    using posting_list_type = decltype(index.postings(std::declval<std::string>()).fetch());
    std::vector<posting_list_type> postings;
    postings.reserve(query_terms.size());
    for (const auto& term : query_terms) {
        postings.push_back(index.postings(term).fetch());
    }
    return postings;
}

template<typename Index>
inline auto
fetched_query_scored_postings(Index const& index, gsl::span<std::string const>& query_terms)
{
    using posting_list_type = decltype(index.scored_postings(std::declval<std::string>()).fetch());
    std::vector<posting_list_type> postings;
    postings.reserve(query_terms.size());
    for (const auto& term : query_terms) {
        postings.push_back(index.scored_postings(term).fetch());
    }
    return postings;
}

struct Printable : boost::te::poly<Printable> {
    using boost::te::poly<Printable>::poly;

    void print(std::ostream& out) const
    {
        boost::te::call([](auto const& self, auto& out) { out << self; }, *this, out);
    }
};
std::ostream& operator<<(std::ostream& os, Printable const& p)
{
    p.print(os);
    return os;
}

struct Query_Result_List {
    std::vector<std::pair<irk::index::document_t, Printable>> results;

    template<class Score>
    Query_Result_List(std::vector<std::pair<irk::index::document_t, Score>> results)
        : self_(std::make_shared<Impl<Score>>(results))
    {}

    void print(std::function<void(int, irk::index::document_t, Printable)> f) const
    {
        self_->print(f);
    }

private:
    struct Result
    {
        Result() = default;
        Result(Result const&) = default;
        Result(Result&&) noexcept = default;
        Result& operator=(Result const&) = default;
        Result& operator=(Result&&) noexcept = default;
        virtual ~Result() = default;
        virtual void print(std::function<void(int, irk::index::document_t, Printable)> f) const = 0;
    };

    template<class Score>
    class Impl : public Result {
    public:
        explicit Impl(std::vector<std::pair<irk::index::document_t, Score>> results)
            : results_(results)
        {}
        void print(std::function<void(int, irk::index::document_t, Printable)> f) const override
        {
            int rank = 0;
            for (auto& result : results_) {
                f(rank++, result.first, result.second);
            }
        }
    private:
        std::vector<std::pair<irk::index::document_t, Score>> results_;
    };

private:
    std::shared_ptr<Result> self_;
};

class Query_Engine {
public:
    template<class Index, class Score_Tag, class Traversal_Tag>
    explicit Query_Engine(Index const& index,
                          bool nostem,
                          Score_Tag score_tag,
                          Traversal_Tag traversal_tag,
                          std::optional<int> trec_id,
                          std::string run_id)
        : self_(std::make_shared<Impl<Index, Score_Tag, Traversal_Tag>>(
              index, nostem, score_tag, traversal_tag, trec_id, std::move(run_id)))
    {}

    [[nodiscard]] Query_Result_List run_query(gsl::span<std::string const> query_terms, int k)
    {
        return self_->run_query(query_terms, k);
    }

    [[nodiscard]] static auto is_quantized(std::string const& name) -> bool
    {
        return std::find(name.begin(), name.end(), '-') != name.end();
    }

    template<typename Index>
    [[nodiscard]] static Query_Engine from(Index const& index,
                                           bool nostem,
                                           std::string const& score_function,
                                           Traversal_Type traversal_type,
                                           std::optional<int> trec_id,
                                           std::string const& run_id)
    {
        if (is_quantized(score_function)) {
            switch (traversal_type) {
            case Traversal_Type::TAAT:
                return Query_Engine(index, nostem, empty_tag, taat_traversal, trec_id, run_id);
            case Traversal_Type::DAAT:
                return Query_Engine(index, nostem, empty_tag, daat_traversal, trec_id, run_id);
            }
            throw std::runtime_error("unknown traversal type");
        } else {
            switch (traversal_type) {
            case Traversal_Type::TAAT:
                if (score_function == "bm25") {
                    return Query_Engine(
                        index, nostem, score::bm25, taat_traversal, trec_id, run_id);
                } else if (score_function == "ql") {
                    return Query_Engine(
                        index, nostem, score::query_likelihood, taat_traversal, trec_id, run_id);
                } else {
                    throw std::runtime_error("unknown score function type");
                }
            case Traversal_Type::DAAT:
                if (score_function == "bm25") {
                    return Query_Engine(
                        index, nostem, score::bm25, taat_traversal, trec_id, run_id);
                } else if (score_function == "ql") {
                    return Query_Engine(
                        index, nostem, score::query_likelihood, taat_traversal, trec_id, run_id);
                } else {
                    throw std::runtime_error("unknown score function type");
                }
            }
            throw std::runtime_error("unknown traversal type");
        }
    }

private:
    struct Engine
    {
        Engine() = default;
        Engine(Engine const&) = default;
        Engine(Engine&&) noexcept = default;
        Engine& operator=(Engine const&) = default;
        Engine& operator=(Engine&&) noexcept = default;
        virtual ~Engine() = default;
        [[nodiscard]] virtual Query_Result_List
        run_query(gsl::span<std::string const> query_terms, int k) = 0;
    };

    template<class Index, class Score_Tag, class Traversal_Tag>
    class Impl : public Engine {
    public:
        explicit Impl(Index const& index,
                      bool nostem,
                      Score_Tag score_tag,
                      Traversal_Tag traversal_tag,
                      std::optional<int> trec_id,
                      std::string run_id)
            : index_(index),
              nostem_(nostem),
              scorer_(score_tag),
              traversal_tag_(traversal_tag),
              trec_id_(trec_id),
              run_id_(std::move(run_id))
        {}

        [[nodiscard]] auto run_query_with_precomputed(gsl::span<std::string const> query_terms,
                                                      int const k,
                                                      Index const& index,
                                                      Traversal_Tag traversal_tag)
        {
            if constexpr (std::is_same_v<Traversal_Tag, irk::Taat_Traveral_Tag>) {
                return irk::taat(gsl::make_span(fetched_query_scored_postings(index, query_terms)),
                                 index.collection_size(),
                                 k);
            } else if constexpr (std::is_same_v<Traversal_Tag, irk::Daat_Traveral_Tag>) {
                return irk::daat(gsl::make_span(fetched_query_scored_postings(index, query_terms)),
                                 k);
            }
            std::clog << "unimplemented traversal tag: " << traversal_tag << '\n';
            std::abort();
        }

        [[nodiscard]] auto run_query_with_scoring(gsl::span<std::string const> query_terms,
                                                  int const k,
                                                  const Index& index,
                                                  Score_Tag score_tag,
                                                  Traversal_Tag traversal_tag)
        {
            const auto scorers = fetch_scorers(index, query_terms, score_tag);
            const auto postings = fetched_query_postings(index, query_terms);
            assert(scorers.size() == postings.size());
            if constexpr (std::is_same_v<Traversal_Tag, irk::Taat_Traveral_Tag>) {
                return irk::taat(
                    gsl::make_span(postings), gsl::make_span(scorers), index.collection_size(), k);
            } else if constexpr (std::is_same_v<Traversal_Tag, irk::Daat_Traveral_Tag>) {
                return irk::daat(gsl::make_span(postings), gsl::make_span(scorers), k);
            }
            std::clog << "unimplemented traversal tag: " << traversal_tag << '\n';
            std::abort();
        }

        template<class Document, class Score>
        void print_results(gsl::span<std::pair<Document, Score>> const& results)
        {
            auto const& titles = index_.titles();
            int rank = 0;
            for (auto& result : results) {
                print_(rank++, result.first, result.second);
            }
        }

        [[nodiscard]] Query_Result_List
        run_query(gsl::span<std::string const> query_terms, int k) override
        {
            std::vector<std::string> stemmed_terms;
            if (not nostem_) {
                irk::porter2_stemmer stem{};
                irk::transform_range(query_terms,
                                     std::back_inserter(stemmed_terms),
                                     [&](auto const& term) { return stem(term); });
            }
            if constexpr (std::is_base_of_v<score::scoring_function_tag, Score_Tag>) {
                auto results = run_query_with_scoring(
                    query_terms, k, index_, scorer_, traversal_tag_);
                return Query_Result_List(std::move(results));
            } else {
                auto results = run_query_with_precomputed(query_terms, k, index_, traversal_tag_);
                return Query_Result_List(std::move(results));
            }
        }

    private:
        Index const& index_;
        bool nostem_;
        Score_Tag scorer_;
        Traversal_Tag traversal_tag_;
        std::optional<int> trec_id_;
        std::string run_id_;
    };

private:
    std::shared_ptr<Engine> self_;
};

}  // namespace irk

template<>
struct fmt::formatter<irk::Printable> : formatter<string_view> {
    template<typename FormatContext>
    auto format(irk::Printable p, FormatContext& ctx)
    {
        std::ostringstream os;
        os << p;
        return formatter<string_view>::format(os.str(), ctx);
    }
};

ext: hpp

max_stars_repo_name: elshize/irkit

max_issues_repo_name: elshize/irkit

max_forks_repo_name: elshize/irkit


text:
// Copyright 2021, Autonomous Space Robotics Lab (ASRL)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/**
 * \file types.hpp
 * \brief Header defining types used in VTR vision package
 * \details
 *
 * \author Autonomous Space Robotics Lab (ASRL)
 */
#pragma once

#include <array>
#include <memory>
#include <vector>

#include <Eigen/Core>
#include <Eigen/StdVector>

// opencv definitions
#include <opencv2/core/core.hpp>
#include <opencv2/core/cuda.hpp>
#include <opencv2/core/version.hpp>  // defines CV_MAJOR_VERSION
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/opencv_modules.hpp>  // defines HAVE_OPENCV_CUDAFEATURES2D

#include <lgmath.hpp>
#include <steam.hpp>

#include <vtr_common/utils/hash.hpp>
#include <vtr_logging/logging.hpp>
#include <vtr_tactic/types.hpp>

#include <vtr_messages/msg/h_vec3.hpp>
#include <vtr_messages/msg/keypoint.hpp>
#include <vtr_messages/msg/match.hpp>
#include <vtr_messages/msg/velocity.hpp>

namespace vtr {
namespace vision {

// this is important for OpenCV and SURF GPU multithreaded operations
extern std::mutex __gpu_mutex__;

////////////////////////////////////////////////////////////////////////////////
// Image storage, the analog of robochunk Images.proto

/// An image from a single channel type and camera
struct Image {
  /** \brief  default constructor
   */
  Image() = default;

  /** \brief copy constructor
   *
   * @param img
   */
  Image(const Image &img) {
    stamp = img.stamp;
    name = img.name;
    data = img.data.clone();
  }

  /** \brief copy assignment (required due to defaulted deletion in the
   * compiler)
   */
  Image &operator=(const Image &img) {
    stamp = img.stamp;
    name = img.name;
    data = img.data.clone();
    return *this;
  }

  /** \brief move constructor
   */
  Image(Image &&img) noexcept {
    stamp = img.stamp;
    name = img.name;
    data = img.data;
  }
  /// The image timestamp (ns epoch time)
  uint64_t stamp;
  /// The name of the camera (e.g. left, right)
  std::string name;
  /// The OpenCV image
  cv::Mat data;
};

/// Images from a camera rig for a single channel type
struct ChannelImages {
  /** \brief default constructor
   */
  ChannelImages() = default;

  /** \brief copy constructor
   */
  ChannelImages(const ChannelImages &channel) {
    name = channel.name;
    cameras = channel.cameras;
  }

  /** \brief copy assignment (required due to defaulted deletion in the
   * compiler)
   */
  ChannelImages &operator=(const ChannelImages &channel) {
    name = channel.name;
    cameras = channel.cameras;
    return *this;
  }

  /** \brief move constructor
   */
  ChannelImages(ChannelImages &&channel) noexcept {
    name = channel.name;
    for (auto &camera : channel.cameras) {
      cameras.emplace_back(std::move(camera));
    }
  }
  /// The name of the channel (e.g. grey, dessert, forest)
  std::string name;
  /// The images from the different cameras in the rig
  std::vector<Image> cameras;
};

/// Images from a camera rig
struct RigImages {
  /** \brief default constructor */
  RigImages() = default;

  /** \brief copy constructor */
  RigImages(const RigImages &channel) {
    name = channel.name;
    channels = channel.channels;
  }

  /**
   * \brief copy assignment (required due to defaulted deletion in the compiler)
   */
  RigImages &operator=(const RigImages &channel) {
    name = channel.name;
    channels = channel.channels;
    return *this;
  }

  /** \brief move constructor */
  RigImages(RigImages &&rig) noexcept {
    name = rig.name;
    for (auto &channel : rig.channels) {
      channels.emplace_back(std::move(channel));
    }
  }
  ~RigImages() = default;
  /// The name of the rig (e.g. front-xb3, rear-visensor)
  std::string name;
  /// The images from all the different channel types
  std::vector<ChannelImages> channels;
};

////////////////////////////////////////////////////////////////////////////////
// Simple matches for interacting with Steam

using SimpleMatch = std::pair<unsigned, unsigned>;
using SimpleMatches = std::vector<SimpleMatch>;

struct ChannelMatches {
  std::string name;
  SimpleMatches matches;
};

struct RigMatches {
  std::string name;
  std::vector<ChannelMatches> channels;
};

typedef std::vector<RigMatches> SuiteMatches;

////////////////////////////////////////////////////////////////////////////////
// Keypoints

/// Use OpenCV's keypoint, since we need to operator on this type
typedef cv::KeyPoint Keypoint;
typedef std::vector<Keypoint> Keypoints;

////////////////////////////////////////////////////////////////////////////////
// Descriptors

enum struct FeatureImpl { UNKNOWN = 0, OPENCV_ORB, ASRL_GPU_SURF };
struct FeatureType {
  /// The implementation used for feature extraction
  FeatureImpl impl;
  /// The dimension of the descriptor
  unsigned dims;
  /// The number of bytes in the descriptor
  unsigned bytes_per_desc;
  /// Whether the descriptor is upright or rotated (rotation invariant)
  bool upright;
};

////////////////////////////////////////////////////////////////////////////////
// Collections of features, mirroring Features.proto but with usable types

/// Extra keypoint appearance info not covered by OpenCV's definition.
/// The split here differs from the protobuf split between keypoint and info.
struct FeatureInfo {
  /// Detector Laplacian bit
  bool laplacian_bit;
  /// Keypoint precision (1/sigma^2), where sigma is in pixels
  float precision;

  Eigen::Matrix2d covariance;
  FeatureInfo(bool lb, float pr) : laplacian_bit(lb), precision(pr) {}
  FeatureInfo() : laplacian_bit(false), precision(0.0f) {}
};
typedef std::vector<FeatureInfo> FeatureInfos;

/// The collection of features from an image for a single channel type
struct Features {
  /// The camera name (e.g. left, right)
  std::string name;

  /// The keypoints detected in the image
  Keypoints keypoints;

  /// The extra keypoint information not included in the OpenCV keypoint
  FeatureInfos feat_infos;

  /// The descriptors stored as a binary blob
  cv::Mat descriptors;

  /// The descriptors stored as a binary blob
#if CV_MAJOR_VERSION >= 3 && defined(HAVE_OPENCV_CUDAFEATURES2D)
  cv::cuda::GpuMat gpu_descriptors;
#endif

  /// The type of feature extracted
  FeatureType feat_type;
};

/// The collection of features from each camera in a rig for one channel type
struct ChannelFeatures {
  /// The channel name (e.g. grey, dessert, forest)
  std::string name;
  /// The features for each camera in the rig
  std::vector<Features> cameras;
  // Matches rig_matches;
  /// Whether the features are fully matched.
  /// This means every feature has a match, and the indices are match aligned.
  bool fully_matched;
};

/// The collection of features for every camera and channel in a camera rig
struct RigFeatures {
  /// The rig name (e.g. front-xb3, rear-visensor)
  std::string name;
  /// The features for each channel in the rig
  std::vector<ChannelFeatures> channels;
};

typedef std::vector<RigFeatures> SuiteFeatures;

////////////////////////////////////////////////////////////////////////////////
// Landmark Matches
struct PersistentId {
  PersistentId() : stamp(-1), robot(-1) {}
  PersistentId(uint64_t s, uint32_t r) : stamp(s), robot(r) {}
  uint64_t stamp;
  uint32_t robot;
};

struct LandmarkId {
  LandmarkId() : index(-1), channel(-1), camera(-1), rig(-1), persistent() {}

  LandmarkId(const PersistentId &p, uint32_t ri, uint32_t ch)
      : index(-1), channel(ch), camera(-1), rig(ri), persistent(p) {}

  // THIS ORDER IS DIFFERENT THAN NORMAL, READ CAREFULLY
  LandmarkId(const PersistentId &p, uint32_t ri, uint32_t ch, uint32_t idx,
             uint32_t cm)
      : index(idx), channel(ch), camera(cm), rig(ri), persistent(p) {}
  uint32_t index;
  uint32_t channel;
  uint32_t camera;
  uint32_t rig;
  PersistentId persistent;
};
typedef std::vector<LandmarkId> LandmarkIds;

inline std::ostream &operator<<(std::ostream &os, const LandmarkId &id) {
  os << "ro" << id.persistent.robot << ",t" << id.persistent.stamp << ",ri"
     << id.rig << ",ch" << id.channel << ",i" << id.index << ",ca" << id.camera;
  return os;
}

// Equality comparison so it can be used in a sorted map
inline bool operator==(const vtr::vision::PersistentId &a,
                       const vtr::vision::PersistentId &b) {
  return a.stamp == b.stamp && a.robot == b.robot;
}

// Equality comparison so it can be used in a sorted map
inline bool operator==(const vtr::vision::LandmarkId &a,
                       const vtr::vision::LandmarkId &b) {
  return a.persistent == b.persistent && a.rig == b.rig &&
         a.camera == b.camera && a.channel == b.channel && a.index == b.index;
}

// Inequality comparison so it can be used in a sorted map.
inline bool operator!=(const vtr::vision::PersistentId &a,
                       const vtr::vision::PersistentId &b) {
  return !(a == b);
}

// Inequality comparison so it can be used in a sorted map.
inline bool operator!=(const vtr::vision::LandmarkId &a,
                       const vtr::vision::LandmarkId &b) {
  return !(a == b);
}

// Inequality comparison so it can be used in a sorted map.
inline bool operator<(const vtr::vision::PersistentId &a,
                      const vtr::vision::PersistentId &b) {
  if (a.robot < b.robot) return true;
  if (a.robot > b.robot) return false;
  if (a.stamp < b.stamp) return true;
  if (a.stamp > b.stamp) return false;
  return false;  // equal
}

// Inequality comparison so it can be used in a sorted map.
inline bool operator>(const vtr::vision::PersistentId &a,
                      const vtr::vision::PersistentId &b) {
  if (a == b) return false;
  if (a < b) return false;
  return true;
}

// Inequality comparison so it can be used in a sorted map.
inline bool operator<(const vtr::vision::LandmarkId &a,
                      const vtr::vision::LandmarkId &b) {
  if (a.persistent < b.persistent) return true;
  if (a.persistent > b.persistent) return false;
  if (a.rig < b.rig) return true;
  if (a.rig > b.rig) return false;
  if (a.camera < b.camera) return true;
  if (a.camera > b.camera) return false;
  if (a.channel < b.channel) return true;
  if (a.channel > b.channel) return false;
  if (a.index < b.index) return true;
  if (a.index > b.index) return false;
  return false;  // equal
}

// Inequality comparison so it can be used in a sorted map.
inline bool operator>(const vtr::vision::LandmarkId &a,
                      const vtr::vision::LandmarkId &b) {
  return b < a && b != a;
}

struct LandmarkMatch {
  LandmarkId from;
  std::vector<LandmarkId> to;
};

typedef std::vector<LandmarkMatch> LandmarkMatches;

////////////////////////////////////////////////////////////////////////////////
// Landmarks

typedef Eigen::Matrix<double, 3, Eigen::Dynamic> Points3_t;
typedef Eigen::Matrix<double, 9, Eigen::Dynamic> Points9_t;

/// The feature landmarks observed by one channel
struct ChannelLandmarks {
  /// The channel name (e.g. grey, cc_desert, cc_forest)
  std::string name;
  /// The 3D point in the first camera's frame
  Points3_t points;
  /// The 3D point covariance (in the first camera's frame)
  Points9_t covariances;
  /// Appearence information (descriptor, precision, laplacian)
  Features appearance;
  /// Matches to previous experiences
  LandmarkMatches matches;
  /// Whether the landmark is valid
  std::vector<bool> valid;
};

/// The feature landmarks observed by a camera rig
struct RigLandmarks {
  /// The name of the rig (e.g. front-xb3, rear-visensor)
  std::string name;
  /// The landmarks associated with each channel
  std::vector<ChannelLandmarks> channels;
};

typedef std::vector<RigLandmarks> SuiteLandmarks;

////////////////////////////////////////////////////////////////////////////////

typedef cv::Point2f Point;
typedef std::vector<Point> Points;
typedef std::vector<float> Precisions;
typedef std::vector<Eigen::Matrix2d> Covariances;
// Observations
struct Observations {
  std::string name;
  Points points;
  Precisions precisions;
  Covariances covariances;
  LandmarkMatches landmarks;
};

struct ChannelObservations {
  std::string name;
  std::vector<Observations> cameras;
  bool fully_matched;
};

struct RigObservations {
  std::string name;
  std::vector<ChannelObservations> channels;
};

////////////////////////////////////////////////////////////////////////////////
// Bag Of Words

// A vocabulary is just a collection of LandmarkIds
typedef std::vector<LandmarkId> ChannelBowVocabulary;
typedef std::vector<ChannelBowVocabulary> RigBowVocabulary;
typedef std::vector<RigBowVocabulary> SuiteBowVocabulary;

// A BoW Descriptor is a map from LandmarkId to a count of how many times it was
// seen Note: zero counts should not be present, to keep things sparse and fast
typedef std::pair<LandmarkId, unsigned> BowWordCount;
typedef std::map<LandmarkId, unsigned> BowDescriptor;

////////////////////////////////////////////////////////////////////////////////
// Calibrations
typedef Eigen::Matrix<double, 5, 1> CameraDistortion;
typedef std::vector<CameraDistortion,
                    Eigen::aligned_allocator<CameraDistortion>>
    CameraDistortions;
typedef Eigen::Matrix<double, 3, 3> CameraIntrinsic;
typedef std::vector<CameraIntrinsic, Eigen::aligned_allocator<CameraIntrinsic>>
    CameraIntrinsics;
typedef Eigen::Matrix<double, 3, 4> CameraProjection;
typedef std::vector<CameraProjection,
                    Eigen::aligned_allocator<CameraProjection>>
    CameraProjections;
typedef lgmath::se3::Transformation Transform;
typedef std::vector<Transform, Eigen::aligned_allocator<Transform>> Transforms;

/// Rigid camera set information.
struct RigCalibration {
  /// Distortion parameters (k0, k1, p0, p1, k3), one for each camera in the
  /// rig.
  CameraDistortions distortions;
  /// Intrinsic camera matrices, one for each camera in the rig.
  CameraIntrinsics intrinsics;
  /** \brief Transform from origin point to each camera
   */
  Transforms extrinsics;
  /** \brief Indicates whether the rig is rectified (true) or more general
   * (false)
   */
  bool rectified;
};

/// IMU information.
struct IMUCalibration {
  /// Accelerometer and gyro biases
  Eigen::Matrix<double, 6, 1> bias;
  /// Transform from origin point to IMU
  Transform extrinsics;
};

/// Rigid camera set + IMU information.
struct InertialRigCalibration {
  /// The camera calibrations
  RigCalibration rig_calibration;
  /// The IMU calibrations
  IMUCalibration imu_calibration;
};

////////////////////////////////////////////////////////////////////////////////

/** \brief Landmarks in a single privileged frame */
struct LandmarkFrame {
  /** \brief Currently observed landmarks, for each rig */
  vision::RigLandmarks landmarks;
  /** \brief corresponding landmark observations */
  vision::RigObservations observations;
};
using LandmarkFrames = std::vector<LandmarkFrame>;

/** \brief collection of pointers to observations and their origins. */
struct LandmarkObs {
  std::vector<vtr_messages::msg::Keypoint> keypoints;
  std::vector<float> precisions;
  std::vector<std::vector<float>> covariances;
  vtr_messages::msg::Match origin_ref;
};

/**
 * \brief collection of pointers to landmarks and their associated steam
 * containers.
 */
struct LandmarkInfo {
  vtr_messages::msg::HVec3 point;
  std::vector<float> covariance;
  uint8_t descriptor;
  uint32_t num_vo_observations;
  steam::se3::LandmarkStateVar::Ptr steam_lm;
  std::vector<LandmarkObs> observations;
  bool valid;
};
using LandmarkMap = std::unordered_map<vision::LandmarkId, LandmarkInfo>;

/** \brief A steam TransformStateVar Wrapper, keeps track of locking */
class SteamPose {
 public:
  /** \brief Default constructor */
  SteamPose() = default;

  /**
   * \brief Constructor
   * \param T The transformation associated with this pose.
   * \param lock_flag Whether this pose should be locked or not.
   */
  SteamPose(tactic::EdgeTransform T, bool lock_flag) : lock(lock_flag) {
    tf_state_var.reset(new steam::se3::TransformStateVar(T));
    tf_state_var->setLock(lock);
    tf_state_eval.reset(new steam::se3::TransformStateEvaluator(tf_state_var));
  }

  /** \brief Sets the transformation. */
  void setTransform(const tactic::EdgeTransform &transform) {
    tf_state_var.reset(new steam::se3::TransformStateVar(transform));
    tf_state_var->setLock(lock);
    tf_state_eval.reset(new steam::se3::TransformStateEvaluator(tf_state_var));
  }

  void setVelocity(Eigen::Matrix<double, 6, 1> &vel) {
    velocity.reset(new steam::VectorSpaceStateVar(vel));
    velocity->setLock(lock);
  }
  /** \brief Sets the lock */
  void setLock(bool lock_flag) {
    lock = lock_flag;
    tf_state_var->setLock(lock);
  }

  bool isLocked() { return lock; }
  /** \brief The steam state variable. */
  steam::se3::TransformStateVar::Ptr tf_state_var;
  steam::se3::TransformStateEvaluator::Ptr tf_state_eval;
  steam::Time time;
  steam::VectorSpaceStateVar::Ptr velocity;
  std::shared_ptr<vtr_messages::msg::Velocity> proto_velocity;

 private:
  /** \brief The lock flag. */
  bool lock;
};
using SteamPoseMap = std::map<pose_graph::VertexId, SteamPose>;

using SensorVehicleTransformMap =
    std::map<pose_graph::VertexId, lgmath::se3::TransformationWithCovariance>;

using MigrationMap = std::unordered_map<vision::LandmarkId, int>;

// experience recognition
using ScoredRids = std::multimap<float, tactic::RunId>;
using ScoredRid = std::pair<float, tactic::RunId>;
/** \brief the BoW cosine distance of the query to each run in the map. */
using ExperienceDifferences = std::map<tactic::RunId, float>;
/** \brief a BoW cosine distance from the query to the run: <run, distance> */
using ExperienceDifference = std::pair<tactic::RunId, float>;
}  // namespace vision

}  // namespace vtr

// custom specialization of std::hash can be injected in namespace std
namespace std {

// hash for feature ids
template <>
struct hash<vtr::vision::LandmarkId> {
  typedef vtr::vision::LandmarkId argument_type;
  typedef std::size_t result_type;
  result_type operator()(argument_type const &lid) const {
    // return std::hash(s.SerializeAsString()); // <- easy but slow
    result_type seed = 0;
    vtr::common::hash_combine(seed, lid.persistent.stamp, lid.persistent.robot,
                              lid.rig, lid.channel, /*lid.camera,*/ lid.index);
    return seed;
  }  // ()
};   // hash
}  // namespace std