/**
	@file SolarSourceModeling
	@brief Class to model the solar source function intensities

	The SolarSourceModeling module will be used to generate a planar fit of
	solar intensities using the solar source function for each SOFIE event.
	This module will be used by SOFIE Level 1 for motion correction.

	  Interface agreed on for Level 1 on March 2, 2008

	@version 1.0

	@author
		- Kelly Teague
		- Greg Paxton

	@copyright
		(©) Copyright 2008 by GATS Inc.
		11864 Canon Blvd., Suite 101, Newport News, VA 23606

		All Rights Reserved. No part of this software or publication may be 
		reproduced, stored in a retrieval system, or transmitted, in any form 
		or by any means, electronic, mechanical, photocopying, recording, or 
		otherwise without the prior written permission of GATS Inc.

	@see		SolarSourceModeling
	@bug	None known
	@history

*/

#include "SolarSourceModeling.h"
#include <fstream>
#include <iostream>
#include <typeinfo>
#include <string>
#include <algorithm>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

//#define DEBUG

using namespace std;

SolarSourceModeling::SolarSourceModeling(
					 const bool mode,                                    
					 const double balanceStart,
					 const double balanceEnd,
					 const std::vector<double> elevationLockdown,
					 const std::vector<double> azimuthLockdown,
					 const std::vector<double> signal,
					 const std::vector<double> time,
					 const double calibrationStart,
					 const double calibrationEnd,
                                    	 const unsigned int offset                                    
					)
{
//    cout << "input lens to solar source module are " << azimuthLockdown.size() << " and " << elevationLockdown.size() << endl;

    isTheDataOk = true;

    _coefA = 0.0;
    _coefB = 0.0;
    _coefC = 0.0;

    sunsetMode = mode;
    if (sunsetMode < 0 || sunsetMode > 1)
    {
    	isTheDataOk = false;
    	return;
    }

    _balStart = balanceStart;
    _balEnd = balanceEnd;
    
    if (_balStart <= 0 || _balEnd <= 0)
    {
    	isTheDataOk = false;
    	return;
    }

    _calStart = calibrationStart;
    _calEnd = calibrationEnd;

    if (_calStart <= 0 || _calEnd <= 0)
    {
    	isTheDataOk = false;
    	return;
    }

    _elLockdown = elevationLockdown;
    _azLockdown = azimuthLockdown;
    _det = signal;
    _t = time;

    if (_elLockdown.size() == 0 || _azLockdown.size() == 0 || _det.size() == 0 || _t.size() == 0)
    {
    	isTheDataOk = false;
    	return;
    }

    _offsetData = offset;

    init();
}

void SolarSourceModeling::init(void)
{
	if (isTheDataOk)
	{
		validateDataValues();
//		cout << "after validation, lens are " << _elLockdown.size() << " and " << _azLockdown.size() << " and " << _det.size() << endl;
	}
	else
	{
		cerr << "SolarSourceModeling ERROR: failed in init() before validateDataValues()" << endl;
		return;
	}

	if (isTheDataOk)
	{
		setCalibrationData();
//		cout << "after calibration, lens are " << _elLockdown.size() << " and " << _azLockdown.size() << " and " << _det.size() << endl;
	}
	else
	{
		cerr << "SolarSourceModeling ERROR: failed in init() before setCalibrationData()" << endl;
		return;
	}

	if (isTheDataOk)
	{
		getCruciformSubsetData();
	}
	else
	{
		cerr << "SolarSourceModeling ERROR: failed in init() before getCruciformSubsetData()" << endl;
		return;
	}

	if (isTheDataOk)
	{
		getCruciformSubsections();
	}
	else
	{
		cerr << "SolarSourceModeling ERROR: failed in init() before getCruciformSubsections()" << endl;
		return;
	}

	if (isTheDataOk && _offsetData)
	{
		getOffsetData();
	}

	if (isTheDataOk)
	{
		setCoefficients();
	}
	else
	{
		cerr << "SolarSourceModeling ERROR: failed in init() before setCoefficients()" << endl;
		return;
	}
}

double SolarSourceModeling::getIntensity(const double elevation, const double azimuth) const
{
	return _coefA * azimuth + _coefB * elevation + _coefC;
}

SolarSourceModeling::~SolarSourceModeling()
{
}

void SolarSourceModeling::validateDataValues()
{
    std::vector<double> newXData; 
    std::vector<double> newYData;
    std::vector<double> newZData;
    std::vector<double> newTimes;

    for (unsigned int i = 0; i < _det.size(); i++)
    {
        if ((_azLockdown[i] < 10000.0) && (_azLockdown[i] > -10000.0) && (_elLockdown[i] < 10000.0) && (_elLockdown[i] > -10000.0))
        {
             newXData.push_back(_elLockdown[i]);
             newYData.push_back(_azLockdown[i]);
             newZData.push_back(_det[i]);
             newTimes.push_back(_t[i]);
        }
    }
    
    _elLockdown = newXData;
    _azLockdown = newYData;
    _det = newZData;
    _t = newTimes;

    if (_elLockdown.size() == 0 || _azLockdown.size() == 0 || _det.size() == 0 || _t.size() == 0)
    {
    	cerr << "SolarSourceModeling ERROR:  At least one vector that was passed in had a zero size." << endl;
    	isTheDataOk = false;
    	return;
    }

    if ((_elLockdown.size() + _azLockdown.size() + _det.size() + _t.size())/4 !=_elLockdown.size()) 
    {
    	cerr << "SolarSourceModeling ERROR:  All vectors passed in did not have the same size." << endl;
    	isTheDataOk = false;
    	return;
    }
}

void SolarSourceModeling::setCalibrationData()
{
	std::vector<double> newXData; 
	std::vector<double> newYData;
	std::vector<double> newZData;
	std::vector<double> newTimes;
	
	calibrationStartIndex = 0; 
	calibrationEndIndex = 0;

	setCalibrationTimes();
	getCalibrationIndices(_t);

	if ((calibrationStartIndex == 0) && (calibrationEndIndex == 0))
	{
		cerr << "SolarSourceModeling ERROR: Unable to resolve calibration times" << endl;
		isTheDataOk = false;
	}	
	else
	{
//		cout << "cal indices are " << calibrationStartIndex << " to " << calibrationEndIndex << endl;
		for (int i = calibrationStartIndex; i < calibrationEndIndex; i++)
		{
	         newXData.push_back(_elLockdown[i]);
	         newYData.push_back(_azLockdown[i]);
	         newZData.push_back(_det[i]);
	         newTimes.push_back(_t[i]);
		} 
	
	    _elLockdown = newXData;
	    _azLockdown = newYData;
	    _det = newZData;
	    _t = newTimes;
	}
}

void SolarSourceModeling::getOffsetData()
{
	if (_offsetData)
	{
		std::vector<double> newData;

		double maxValue = getMaxValue(_det);
		for (unsigned int i = 0 ; i < _det.size(); i++)
		{
			newData.push_back(_det[i] - maxValue); 
		}
		_det = newData;
	}
}

double SolarSourceModeling::getMaxValue(std::vector<double> data)
{
	double maxData = 0;

    for (unsigned int i = 0; i < data.size(); i++)
    {
    	if (data[i] > maxData) 
    	{
			maxData = data[i];
    	}
    }
    return maxData;
}


void SolarSourceModeling::setCalibrationTimes()
{

    if ((_balStart - _calStart) < (_calEnd - _balEnd))
    {
        _relCalStartTime = _balEnd;
        _relCalEndTime = _calEnd;
    }

    if ((_balStart - _calStart) > (_calEnd - _balEnd))
    {
        _relCalStartTime = _calStart;
        _relCalEndTime = _balStart;
    }
}

void SolarSourceModeling::getCalibrationIndices(std::vector<double> data)
{
    calibrationStartIndex = 0;
    calibrationEndIndex = data.size()-1;

//    cout << "cal start time = " << _relCalStartTime << " cal end time = " << _relCalEndTime << endl;
    for (unsigned int i = 0; i < data.size(); i++)
    {
//	cout << i << ": " << data[i] << endl;
    	if ((_relCalStartTime <= data[i]) && (calibrationStartIndex == 0)) 
    	{
		calibrationStartIndex = i;
    	}

    	if (_relCalEndTime <= data[i]) 
    	{
    		calibrationEndIndex = i;
		return;
    	}
    
//	cout << i << ": calibrationStartIndex = " << calibrationStartIndex << ". calibrationEndIndex = " << calibrationEndIndex << endl;
    }
}

std::vector<double> SolarSourceModeling::getCruciformStart()
{
    double numberOfPoints = 20;
    double startAz = 0;
    double startEl = 0;

//    cout << "input lens to cruciform start are " << _azLockdown.size() << " and " << _elLockdown.size() << endl;
    for (unsigned int i = 0; i < numberOfPoints; i++)
    {
	startAz += _azLockdown[i];
	startEl += _elLockdown[i];
//	cout << "summing " << _azLockdown[i] << " = " << startAz << ". " << _elLockdown[i] << " = " << startEl << endl;
    }

//    cout << "startAz sum = " << startAz << ". startEl sum = " << startEl << endl;

    startAz /= numberOfPoints;
    startEl /= numberOfPoints;

//    cout << "startAz = " << startAz << ". startEl = " << startEl << endl;

    std::vector<double> cruciformStart;
    cruciformStart.push_back(startAz);
    cruciformStart.push_back(startEl);
    
    return cruciformStart;
}


void SolarSourceModeling::getCruciformSubsections(void)
{
    double tolerance = 0.0001696847883883; //35arcsecs

    if (sunsetMode)        //(_eventNo % 2) > 0
    {
        std::reverse(_azLockdown.begin(), _azLockdown.end());
        std::reverse(_elLockdown.begin(), _elLockdown.end());
        std::reverse(_det.begin(), _det.end());
    }

    double startAz = _azLockdown[0];
    double startEl = _elLockdown[0];

    bool azFlag1 = 0;
    bool azFlag2 = 0;
    bool elFlag = 0;

    unsigned int azStart1 = 0;
    unsigned int azEnd1 = 0;
    unsigned int azStart2 = 0;
    unsigned int azEnd2 = _azLockdown.size();
    unsigned int elStart = 0;
    unsigned int elEnd = _elLockdown.size();

//    cout << "getCruciformSubsections" << endl;
//    cout << "Az-tolerance,az,Az+tolerance,azFlag1,azStart1,azEnd1,azFlag2,azStart2,azEnd2,";
//    cout << "El-tolerance,el,El+tolerance,elFlag,elStart,elEnd" << endl;

    for (unsigned int i = 0; i < _azLockdown.size(); i++)
    {
//	cout << startAz-tolerance << "," << _azLockdown[i] << "," << startAz+tolerance << ",";
//	cout << azFlag1 << "," << azStart1 << "," << azEnd1 << "," << azFlag2 << "," << azStart2 << "," << azEnd2 << ",";
//	cout << startEl-tolerance << "," << _elLockdown[i] << "," << startEl+tolerance << ",";
//	cout << elFlag << "," << elStart << "," << elEnd << endl;

//	cout << "start 1: " << startAz-tolerance << " < " << _azLockdown[i] << "(" << ((startAz-tolerance) < _azLockdown[i]) << ") && " << azStart1 << " == 0 (" << (azStart1 == 0) << ")" << endl;
		if (azFlag1 == 0)
		{
	    	//use Az data to find indices for the first subsection of Az data

	    	if (((startAz-tolerance) < _azLockdown[i]) && (azStart1 == 0))
	    	{
				azStart1 = i;
	    	}
	    	if ((startAz+tolerance) < _azLockdown[i])
	    	{
				azEnd1 = i;
				azFlag1 = 1;
	    	}
		}
		else
		{
	    	//indices for the first subsection of Az data assigned
	    	//use Az data to find indices for the second subsection of Az data

	    	if (azFlag2 == 0)
	    	{
				if (((startAz+tolerance) > _azLockdown[i]) && (azStart2 == 0))
				{
		    		azStart2 = i;
				}
				if ((startAz-tolerance) > _azLockdown[i])
				{
		    		azEnd2 = i;
		    		azFlag2 = 1;
				}
	    	}
	    	else
	    	{
				//indices for the second subsection of Az data assigned
				//use El data to find indices for the final subsection of El data

				if (elFlag == 0)
				{
		    		if ((startEl-tolerance) > _elLockdown[i])
		    		{
						elFlag = 1;
		    		}
				}
				else
				{
		    		if ((startEl-tolerance) < _elLockdown[i] )
		    		{
						elStart = i;
						elFlag = 0;
		    		}
		    		if ((startEl+tolerance) < _elLockdown[i] )
		    		{
						elEnd = i;
		    		}
				}
	    	}
		}
    }

	// Inserted by GJP on 4/2/2010
	if (!azFlag1 || !azFlag2)
	{
		isTheDataOk = false;
		return;
	}



//    cout << "subsection1 data from " << azStart1 << " to " << azEnd1 << endl;
//    cout << "subsection2 data from " << azStart2 << " to " << azEnd2 << endl;
//    cout << "subsection3 data from " << elStart << " to " << elEnd << endl;

    std::vector<double> newXData;
    std::vector<double> newYData;
    std::vector<double> newZData;

//    cout << endl << "section1" << endl;

    //append the first subsection of data
    for (unsigned int i = azStart1; i < azEnd1; i++)
    {
//	cout << _azLockdown[i] << "," << _elLockdown[i] << "," << _det[i] << endl;
	_elData1.push_back(_elLockdown[i]);
	_azData1.push_back(_azLockdown[i]);
	_intData1.push_back(_det[i]);

	newXData.push_back(_elLockdown[i]);
	newYData.push_back(_azLockdown[i]);
	newZData.push_back(_det[i]);
    }

//    cout << endl << "section2" << endl;

    //append the second subsection of data
    for (unsigned int i = azStart2; i < azEnd2; i++)
    {
//	cout << _azLockdown[i] << "," << _elLockdown[i] << "," << _det[i] << endl;
	_elData2.push_back(_elLockdown[i]);
	_azData2.push_back(_azLockdown[i]);
	_intData2.push_back(_det[i]);

	newXData.push_back(_elLockdown[i]);
	newYData.push_back(_azLockdown[i]);
	newZData.push_back(_det[i]);
    }

//    cout << endl << "section3" << endl;

    //append the final subsection of data
    for (unsigned int i = elStart; i < elEnd; i++)
    {
//	cout << _azLockdown[i] << "," << _elLockdown[i] << "," << _det[i] << endl;
	_elData3.push_back(_elLockdown[i]);
	_azData3.push_back(_azLockdown[i]);
	_intData3.push_back(_det[i]);

	newXData.push_back(_elLockdown[i]);
	newYData.push_back(_azLockdown[i]);
	newZData.push_back(_det[i]);
    }

    //normalize here
    std::vector<double> normalizedIntensity = normalizeIntensity();

    _elLockdown.clear();
    _azLockdown.clear();
    _det.clear();

    _elLockdown = newXData;
    _azLockdown = newYData;
    _det = normalizedIntensity;

    if (sunsetMode)        //(_eventNo % 2) > 0
    {
        std::reverse(_azLockdown.begin(), _azLockdown.end());
        std::reverse(_elLockdown.begin(), _elLockdown.end());
        std::reverse(_det.begin(), _det.end());
    }
//    cout << "at end of getCruciformSubsections, lens are " << _azLockdown.size() << " " <<_elLockdown.size() << " " <<_det.size() << endl; 
}


//subsectioned az,el,int data is already reversed for sunsets
//final normalized intensity reversed to correct direction for sunsets
std::vector<double> SolarSourceModeling::normalizeIntensity(void)
{
    double centerAz1 = _azData1[0];
    double centerEl1 = _elData1[0];
    double centerInt1 = _intData1[0];

    int index2 = getIndexOfNearestIntensity(centerAz1, centerEl1, _azData2, _elData2);
    int index3 = getIndexOfNearestIntensity(centerAz1, centerEl1, _azData3, _elData3);

//    cout << "normalizeIntensity: index2 = " << index2 << " and index3 = " << index3 << endl;

    double ratio2 = 0;

    if (index2 > -1)
    {
	ratio2 = centerInt1 / _intData2[index2];
    } 
    else
    {
        cerr << "SolarSourceModeling ERROR: failed in normalizeIntensity() while trying to normalize subsection 2" << endl;
        isTheDataOk = false;
    }

    double ratio3 = 0;
    if (index3 > -1)
    {
	ratio3 = centerInt1 / _intData3[index3];
    } 
    else
    {
        cerr << "SolarSourceModeling ERROR: failed in normalizeIntensity() while trying to normalize subsection 3" << endl;
        isTheDataOk = false;
    }

//    cout << "normalizeIntensity: ratio2 = " << ratio2 << " and ratio3 = "<< ratio3 << endl;

    std::vector<double> normalizedIntensity = _intData1;

    for (unsigned int i = 0; i < _intData2.size(); i++)
    {
	normalizedIntensity.push_back(_intData2[i] * ratio2);
    }
 
    for (unsigned int i = 0; i < _intData3.size(); i++)
    {
	normalizedIntensity.push_back(_intData3[i] * ratio3);
    }

//    cout << "normalizeIntensity: final det len = " << normalizedIntensity.size() << endl;
    return normalizedIntensity;
}

int SolarSourceModeling::getIndexOfNearestIntensity(double x0, double y0, std::vector<double> xData, std::vector<double> yData)
{
    double min = 99999999;
    int minIndex = -1;

    double xLen = 0;
    double yLen = 0;
    double lineLen = 0;

    for (unsigned int i = 0; i < _intData3.size(); i++)
    {
	xLen = (xData[i]-x0)*(xData[i]-x0);
	yLen = (yData[i]-y0)*(yData[i]-y0);

	lineLen = sqrt(xLen + yLen);
	if (lineLen < min)
	{
	    min = lineLen;
	    minIndex = i;
	}
    }

    return minIndex;
}

void SolarSourceModeling::getCruciformSubsetData(void)
{
    std::vector<double> cruciformStart;
    std::vector<double> newXData; 
    std::vector<double> newYData;
    std::vector<double> newZData;
    double startAz = 0.0;
    double startEl = 0.0;
    //double tolerance = 0.0001696847883883; //35arcsecs
    double tolerance = 0.0001696847883883; //50arcsecs

    if (sunsetMode)        //(_eventNo % 2) > 0
    {
	std::reverse(_azLockdown.begin(), _azLockdown.end());
	std::reverse(_elLockdown.begin(), _elLockdown.end());
	std::reverse(_det.begin(), _det.end());
    }

    int azStartIndex = getAzStart();
    if (azStartIndex == -1)
    {
	cerr << "SolarSourceModeling ERROR: failed in getCruciformSubsetData() while finding az starting point" << endl;
	isTheDataOk = false;
	return;
    }
    else
    {
	startAz = _azLockdown[azStartIndex];
	startEl = _elLockdown[azStartIndex];

//	cout << "starting at  az = "<< startAz << " and el = "<< startEl << endl;

    	unsigned int startIndex = 0;
    	unsigned int endIndex = 0;

    	int maxAzToleranceFlag = 0;
    	int maxElToleranceFlag = 0;

    	for (unsigned int i = 0; i < _azLockdown.size(); i++)
	{
	    if (!(startEl+tolerance > _elLockdown[i]) && maxElToleranceFlag == 0) 
	    {
		maxElToleranceFlag = 1;
		endIndex = i;
	    }

	    if (!(startAz+tolerance > _azLockdown[i]) && maxAzToleranceFlag == 0) 
	    {
		maxAzToleranceFlag = 1;
		startIndex = i-20;
	    }
	}

//        cout << "startIndex = " << startIndex << ". elIndex = " << endIndex << endl;

	for (unsigned int i = startIndex; i < endIndex; i++)
	{
	    newXData.push_back(_elLockdown[i]);
	    newYData.push_back(_azLockdown[i]);
	    newZData.push_back(_det[i]);
	}

	if (sunsetMode)        //(_eventNo % 2) > 0
	{
	    std::reverse(newXData.begin(), newXData.end());
	    std::reverse(newYData.begin(), newYData.end());
	    std::reverse(newZData.begin(), newZData.end());
	}

	_elLockdown = newXData;
	_azLockdown = newYData;
	_det = newZData;

//	cout << "final el size = " << _elLockdown.size() << ". az size = " << _azLockdown.size() << ". int size = " << _det.size() << endl;
    }
}


int SolarSourceModeling::getAzStart(void)
{
   int azStartIndex = -1;
   double diff = 0;

   for (unsigned int i = 0; i < (_azLockdown.size()-10); i++)
   {
	diff = _azLockdown[i+10] - _azLockdown[i];
	if (diff > 0.0001)
	{
	    azStartIndex = i-40;
	    if (azStartIndex < 0)
	    {
		return 0;
	    }
	    else
	    {
//		cout << "getAzStart: returning index = " << azStartIndex << endl;
		return azStartIndex;
	    }
	}
   }
//   cout << "Could not find index: returning -1 from getAzStart" << endl;
   return -1;
}

double SolarSourceModeling::sum(std::vector<double> data)
{
    double sum = 0;
    for (unsigned int i = 0; i < data.size(); i++)
    {
	sum += data[i];
    }
    return sum;
}

double SolarSourceModeling::sumOfSquares(std::vector<double> data)
{
    double sum = 0;
    for (unsigned int i = 0; i < data.size(); i++)
    {
	sum += pow(data[i],2);
    }
    return sum;
}    

double SolarSourceModeling::sumCombined1d(std::vector<double> xData, std::vector<double> yData)
{
    double sum = 0;
    for (unsigned int i = 0; i < xData.size(); i++)
    {
	sum += xData[i]*yData[i];
    }
    return sum;
}

void SolarSourceModeling::setCoefficients()
{
	double matrix1[3][3];
	double matrix2[3];

	double total = _det.size();
	double determinant = 0;
	double Sxx = 0; 
	double Syy = 0; 
	double Sxy = 0; 
	double Sxi = 0; 
	double Syi = 0; 
	double Si = 0;
	double Sx = 0;
	double Sy = 0;
	
	Si = sum(_det); 

    Sx = sum(_azLockdown);
    Sy = sum(_elLockdown);
        
    Sxx = sumOfSquares(_azLockdown);
    Syy = sumOfSquares(_elLockdown);
        
	Sxy = sumCombined1d(_azLockdown,_elLockdown);
    Sxi = sumCombined1d(_azLockdown,_det);
    Syi = sumCombined1d(_elLockdown,_det);

    matrix1[0][0] = (total*Syy) - (Sy*Sy);
    matrix1[0][1] = (Sx*Sy) - (total*Sxy);
    matrix1[0][2] = (Sxy*Sy) - (Sx*Syy);
	
    matrix1[1][0] = Sx*Sy - total*Sxy;
    matrix1[1][1] = total*Sxx - Sx*Sx;
    matrix1[1][2] = Sx*Sxy - Sxx*Sy;

    matrix1[2][0] = Sxy*Sy - Sx*Syy;
    matrix1[2][1] = Sx*Sxy - Sxx*Sy;
    matrix1[2][2] = Sxx*Syy - Sxy*Sxy;

	matrix2[0] = Sxi;
	matrix2[1] = Syi;
	matrix2[2] = Si;
	
    determinant = Sxx*(total*Syy - Sy*Sy) - Sxy*(total*Sxy - Sx*Sy) + Sx*(Sxy*Sy - Sx*Syy);
    if (determinant == 0)
    {
    	cerr << "SolarSourceModeling ERROR:  Divide by zero error" << endl;
    	isTheDataOk = false;
    	return;
    }

    double *dotProduct = dot(matrix1, matrix2);

    _coefA = dotProduct[0] / determinant;
    _coefB = dotProduct[1] / determinant;
    _coefC = dotProduct[2] / determinant;

//    cout << "a = " << _coefA << " b = " << _coefB << " c = " << _coefC << endl;
}



double* SolarSourceModeling::dot(double X[][3], double *Y)
{
	double *element = new double[3];
	double sum = 0;
	for(unsigned int i = 0; i < 3; i++)
	{
		for(unsigned int j = 0; j < 3; j++)
		{
			sum += X[i][j] * Y[j]; 
		}
		element[i] = sum;
		sum = 0;
	}

	return element;
}