#include <math.h>

#ifdef HAS_OPENCV3
#include <opencv2/core.hpp> //Any OPENCV3 code
#else
#include <opencv2/core/core.hpp> //Any Opencv2 code
#endif
using namespace cv;
#include <cxxtest/TestSuite.h>

#include "geometry.h"

class TestChangeCoord : public CxxTest::TestSuite
{
    public:
        void testOrigin(void)
        {
            Point origin = Point(1,2);
            //the angle here is actually irrelevant
            Point p = map_point(M_PI/2, origin, origin);
            TS_ASSERT_EQUALS(p.x, 0);
            TS_ASSERT_EQUALS(p.y, 0);
        }

        void testOnlyTranslation(void)
        {
            Point origin = Point(1,2);
            Point p = map_point(0, origin, Point(0,0));
            TS_ASSERT_EQUALS(p.x, -1);
            TS_ASSERT_EQUALS(p.y, -2);
        }

        void testOnlyRotationOrigin(void)
        {
            Point origin = Point(0,0);
            Point p = map_point(M_PI, origin, origin);
            TS_ASSERT_EQUALS(p.x, 0);
            TS_ASSERT_EQUALS(p.y, 0);
        }

        void testOnlyRotation180(void)
        {
            Point origin = Point(0,0);
            Point p = map_point(M_PI, origin, Point(1,2));
            TS_ASSERT_EQUALS(p.x, -1);
            TS_ASSERT_EQUALS(p.y, -2);
        }

        void testOnlyRotation90(void)
        {
            Point origin = Point(0,0);
            Point p = map_point(M_PI/2, origin, Point(1,2));
            TS_ASSERT_EQUALS(p.x, 2);
            TS_ASSERT_EQUALS(p.y, -1);
        }

        void testOnlyRotation30(void)
        {
            Point origin = Point(0,0);
            Point p = map_point(M_PI/6, origin, Point(200,0));
            TS_ASSERT_EQUALS(p.y, -100);
            TS_ASSERT_LESS_THAN(p.x, 180); //0.866*2 ~= 1.73
            TS_ASSERT_LESS_THAN(170, p.x); //0.866*2 ~= 1.73
        }
};

class TestChangeBack : public CxxTest::TestSuite
{
    public:
        void testOrigin(void)
        {
            Point origin = Point(1,2);
            Point p = unmap_point(M_PI/2, origin, Point(0,0));
            TS_ASSERT_EQUALS(p.x, 1);
            TS_ASSERT_EQUALS(p.y, 2);
        }

        void testOnlyTranslation(void)
        {
            Point origin = Point(1,2);
            Point p = unmap_point(0, origin, Point(-1,-2));
            TS_ASSERT_EQUALS(p.x, 0);
            TS_ASSERT_EQUALS(p.y, 0);
        }

        void testOnlyRotationOrigin(void)
        {
            Point origin = Point(0,0);
            Point p = unmap_point(M_PI, origin, origin);
            TS_ASSERT_EQUALS(p.x, 0);
            TS_ASSERT_EQUALS(p.y, 0);
        }

        void testOnlyRotation180(void)
        {
            Point origin = Point(0,0);
            Point p = unmap_point(M_PI, origin, Point(1,2));
            TS_ASSERT_EQUALS(p.x, -1);
            TS_ASSERT_EQUALS(p.y, -2);
        }

        void testOnlyRotation90(void)
        {
            Point origin = Point(0,0);
            Point p = unmap_point(M_PI/2, origin, Point(-2,1));
            TS_ASSERT_EQUALS(p.x, -1);
            TS_ASSERT_EQUALS(p.y, -2);
        }

        void testOnlyRotation30(void)
        {
            Point origin = Point(0,0);
            Point p = unmap_point(M_PI/6, origin, Point(200,0));
            TS_ASSERT_LESS_THAN(p.x, 175); //0.866*2 ~= 1.73
            TS_ASSERT_LESS_THAN(170, p.x); //0.866*2 ~= 1.73
            TS_ASSERT_EQUALS(100, p.y);
        }
};

class TestCoordSystem : public CxxTest::TestSuite
{
    public:
        void testCreate(void)
        {
            auto cs = CoordinateSystem(Point(100,200), Point(200,300));
            TS_ASSERT_EQUALS(cs.map(Point(300,400)).y, 0);
            TS_ASSERT_EQUALS(cs.map(Point(200,100)).x, 0); //moving orthogonally
        }
        void testInverse(void)
        {
            CoordinateSystem(Point(200,300), Point(100,200));
        }
};