#include "stdafx.h"
#include "token.h"
#include "eye.h"
#include <math.h>

Eye::Eye()
{
    SetWidth(0);
    SetHeight(0);
    Point p(0,0,0);
    Init(p, 0, 0, 0, 1);
}


void Eye::Init(const Point& p, double right, double down, double clockwise, double zoom)
{
    _zoom = zoom;
    _m[0]._x = 1.0;  _m[0]._y = 0.0;  _m[0]._z = 0.0;
    _m[1]._x = 0.0;  _m[1]._y = 1.0;  _m[1]._z = 0.0;
    _m[2]._x = 0.0;  _m[2]._y = 0.0;  _m[2]._z = 1.0;
    _m[3].Zero();
    _m[4] = p;
    RotateRight(right);
    RotateDown(down);
    Clockwise(clockwise);
}

void Eye::ReadConfig(const char* spec, bool ring)
{
    Tokenizer t;
    ASSERT(t.InitFromString(spec));
    Point p;
    ASSERT(1 == sscanf_s(t.Text(), "%lg", &p._z));
    p._z = -p._z;
    ASSERT(t.Next());
    double right;
    ASSERT(1 == sscanf_s(t.Text(), "%lg", &right));
    ASSERT(t.Next());
    double down;
    ASSERT(1 == sscanf_s(t.Text(), "%lg", &down));
    ASSERT(t.Next());
    double clockwise;
    ASSERT(1 == sscanf_s(t.Text(), "%lg", &clockwise));
    ASSERT(t.Next());
    double zoom;
    ASSERT(1 == sscanf_s(t.Text(), "%lg", &zoom));
    ASSERT(!t.Next());
	if (ring)
	{
		right = 0;
		down = 0;
	}
    Init(p, right, down, clockwise, zoom);
}


void Eye::ShiftRight(int r)
{
	_m[4]._x += r*_m[4]._z;
}


void Eye::ShiftDown(int n)
{
	_m[4]._y += n*_m[4]._z;
}


// rotate right by l radians
void Eye::RotateRight(double r)
{
    double d[3][3];
	double sinr = sin(r);
	double cosr = cos(r);
    d[0][0] =  cosr; d[0][1] = 0.0; d[0][2] = sinr;
    d[1][0] =  0.0;  d[1][1] = 1.0; d[1][2] = 0.0;
    d[2][0] = -sinr; d[2][1] = 0.0; d[2][2] = cosr;
    Rotate(d);
}

// rotate down by n radians
void Eye::RotateDown(double n)
{
    double d[3][3];
	double sinn = sin(n);
	double cosn = cos(n);
    d[0][0] = 1.0; d[0][1] =  0.0;  d[0][2] = 0.0;
    d[1][0] = 0.0; d[1][1] =  cosn; d[1][2] = sinn;
    d[2][0] = 0.0; d[2][1] = -sinn; d[2][2] = cosn;
    Rotate(d);
}

// rotate clockwise by c radians
void Eye::Clockwise(double c)
{
    double d[3][3];
	double sinc = sin(c);
	double cosc = cos(c);
    d[0][0] =  cosc; d[0][1] = sinc; d[0][2] = 0.0;
    d[1][0] = -sinc; d[1][1] = cosc; d[1][2] = 0.0;
    d[2][0] =  0.0;  d[2][1] = 0.0;  d[2][2] = 1.0;
    Rotate(d);
}

// Apply a rotation: m = D times M-transpose
void Eye::Rotate(const double (&d)[3][3])
{
    Point r[3];
    r[0]._x = _m[0]._x*d[0][0] + _m[1]._x*d[0][1] + _m[2]._x*d[0][2];
    r[0]._y = _m[0]._y*d[0][0] + _m[1]._y*d[0][1] + _m[2]._y*d[0][2];
    r[0]._z = _m[0]._z*d[0][0] + _m[1]._z*d[0][1] + _m[2]._z*d[0][2];
    r[1]._x = _m[0]._x*d[1][0] + _m[1]._x*d[1][1] + _m[2]._x*d[1][2];
    r[1]._y = _m[0]._y*d[1][0] + _m[1]._y*d[1][1] + _m[2]._y*d[1][2];
    r[1]._z = _m[0]._z*d[1][0] + _m[1]._z*d[1][1] + _m[2]._z*d[1][2];
    r[2]._x = _m[0]._x*d[2][0] + _m[1]._x*d[2][1] + _m[2]._x*d[2][2];
    r[2]._y = _m[0]._y*d[2][0] + _m[1]._y*d[2][1] + _m[2]._y*d[2][2];
    r[2]._z = _m[0]._z*d[2][0] + _m[1]._z*d[2][1] + _m[2]._z*d[2][2];

    _m[0] = r[0];
    _m[1] = r[1];
    _m[2] = r[2];
}

// map a point from simulation coordinates to the eye's coordinates
Point Eye::Map(const Point& p)
{
    Point a(p);
    Point b;
    a -= _m[3];
    b._x = a.Dot(_m[0]);
    b._y = a.Dot(_m[1]);
    b._z = a.Dot(_m[2]);
    b -= _m[4];
    return b;
}

Point Eye::MapRing(const Point& p)
{
	Point a(p);
	Point b;

	a -= _m[3];
	
	// convert to a z=0, x >= 0 cross-section of a ring around the x=0 z=0 axis
	a._x = sqrt(a._x*a._x + a._z*a._z);
	a._z = 0;

	b._x = a.Dot(_m[0]);
	b._y = a.Dot(_m[1]);
	b._z = a.Dot(_m[2]);
	b -= _m[4];
	return b;
}