#include "Calibration.h"
#include "ActualData.h"
#include <iostream>
#include <algorithm>
#include <cmath>

namespace math1d_cl
{
	Calibration::Calibration(std::shared_ptr<math1d_cl::MatData> matData, double w, double tolErr)
	{
		this->m_matData = matData;
		this->m_w = w;
		this->m_tolErr = tolErr;
	}

	void Calibration::Calibrate()
	{
			// Init logger
			this->m_logger = new Logger(0,"");

		
			
			// Init members
			this->m_options = this->m_matData->getOptions(); 
			std::vector<std::shared_ptr<math1d_cl::Station>> measuredStations = this->m_matData->getMeasureStations();
			std::vector<std::shared_ptr<math1d_cl::Station>> riverStations = this->m_matData->getRiverStations();
			std::vector<std::shared_ptr<math1d_cl::Channel>> channels = this->m_matData->getChannels();
			std::vector<int> measuredChannelsIdx;
		
			// Check if model has been run
			if(measuredStations.size() == 0)
			{
				m_logger->log("No measured stations, you have to run the model first.");
				std::cerr << "No measured stations, you have to run the model first.\n";
				std::exit(-1);
			}

			// Set limits
			Limit cnLimit, nLimit;
			cnLimit.lower = 30;
			cnLimit.upper = 100;
			nLimit.lower = 0.005;
			nLimit.upper = 0.1;

			// Get measured channels via measured stations
			
			for(size_t i = 0; i < measuredStations.size(); i++)
			{
				m_logger->log("Measured station: " +  measuredStations[i]->getName());
				measuredChannelsIdx.push_back(measuredStations[i]->getChannelIndex());
				// Get initial error for each channel
				m_errBefore.push_back(this->mc_error(this->m_matData, m_matData->getChannels()[measuredChannelsIdx[i]],(int)i,0));
			}

			std::shared_ptr<math1d_cl::MatData> rMatData(new math1d_cl::MatData(*(m_matData.get())));
			
			for(size_t j = 0; j < measuredChannelsIdx.size(); j++)
			{
				m_logger->log("Measured channel ID: " + std::to_string(m_matData->getChannels()[measuredChannelsIdx[j]]->getId()) +  "\nInitial error: "  + std::to_string(m_errBefore[j]));
				// Get upstream channels
				std::vector<int> upstreamChannels = upstream_channels(rMatData->getChannels(), measuredChannelsIdx[j]);
				
				// Prepare actual data
				std::shared_ptr<ActualData> aData (new ActualData(
														rMatData,
														rMatData->getChannels()[measuredChannelsIdx[j]],
														measuredChannelsIdx[j],
														measuredStations[j],
														(int)j,
														upstreamChannels,
														0.05 /*stepCn*/,
														0.01 /*stepN*/,
														1.0 /*p*/,
														cnLimit,
														nLimit
														));
				
				// Get error for loop checking
				double nErr = mc_error(rMatData, aData->getamChannel(),(int)j,this->m_w);
				double aErr = nErr + 2*this->m_tolErr;
				int minCount = 0;
				std::shared_ptr<math1d_cl::MatData> bMatData;
				while(aErr - nErr > m_tolErr)
				{
					aErr = nErr;
					bMatData.reset(new math1d_cl::MatData(*(rMatData.get())));
					//printAll(bMatData);
					// Derive Cn
					std::vector<double> der_cn = derive_cn(rMatData, aData, m_options);
					aData->setCnDerivation(*(new std::shared_ptr<std::vector<double>>(&der_cn)));
					//printAll(rMatData);
					// Derive N
					std::vector<double> der_n = derive_n(rMatData, aData, m_options);
					aData->setnDerivation(*(new std::shared_ptr<std::vector<double>>(&der_n)));
					//printAll(rMatData);
					// Minimalization
					nErr = min_fs(rMatData, aData);
					//printAll(rMatData);
					
					// Cout Err
					m_logger->log("Iteration " + std::to_string(++minCount) + " error " + std::to_string(nErr));
				}

				if(aErr < nErr)
				{
					rMatData.reset(new math1d_cl::MatData(*bMatData));
				}
			
			}

			// Final run of a model
			rMatData->rainfallRunoffModel();

			// Get the error after calibration
			for (size_t k = 0; k < measuredChannelsIdx.size(); k++)
			{
				m_errAfter.push_back(mc_error(rMatData, rMatData->getChannels()[measuredChannelsIdx[k]],(int) k, m_w - m_w /* Weight is ZERO */));
				//m_logger->log("Initial error " + std::to_string(m_errBefore[k]) + " after calibration " + std::to_string(m_errAfter[k]));
				std::cout << "Initial error " << std::to_string(m_errBefore[k]) << " after calibration " << std::to_string(m_errAfter[k]) << std::endl;
			}
	}

	std::vector<double> Calibration::derive_cn(std::shared_ptr<math1d_cl::MatData> matData, std::shared_ptr<ActualData> aData, math1d_cl::Options modelOptions)
	{
		m_logger->log("[Derivation by CN]");
		std::shared_ptr<math1d_cl::MatData> dMatData;
		std::vector<double> der;
		for (size_t i = 0; i < aData->getCnChannelsIdx().size(); i++)
		{
			// Get local copy of matData
			dMatData.reset(new math1d_cl::MatData(*matData));
			
			// Compute partial derivation by Cn
			int currentCnChannelIdx = aData->getCnChannelsIdx()[i];
			std::shared_ptr<math1d_cl::Subbasin> dSubbasin = dMatData->getSubbasins()[currentCnChannelIdx];
			std::shared_ptr<math1d_cl::Subbasin> originalSubbasin = matData->getSubbasins()[currentCnChannelIdx];
			dSubbasin->setCn(originalSubbasin->getCn() + aData->getP());
			//m_logger->log("    Subbasin ID " + std::to_string(originalSubbasin->getId()) + " CN " +  std::to_string(dSubbasin->getCn()));
		
			// Run the model
			dMatData->rainfallRunoffModel();

			// Get results and compute derivation
			std::vector<double> comp1Q = dMatData->getChannels()[aData->getAmChannelIdx()]->getHydrograph().getQOut();
			std::vector<double> comp2Q = matData->getChannels()[aData->getAmChannelIdx()]->getHydrograph().getQOut();
			std::vector<double> measQ = matData->getMeasuredHydrographsQ()[aData->getAmStationIdx()];
			trim_vectors(comp1Q, measQ);
			trim_vectors(comp2Q, measQ);
			der.push_back((norma(measQ,comp1Q,m_w)-norma(measQ,comp2Q,m_w))/aData->getP());
			//	 der(i) = (norma(measQ,comp1Q,w)-norma(measQ,comp2Q,w))/p;
			dMatData.reset();
		}
		return der;
	}

	std::vector<double> Calibration::derive_n(std::shared_ptr<math1d_cl::MatData> matData, std::shared_ptr<ActualData> aData, math1d_cl::Options modelOptions)
	{   
		m_logger->log("[Derivation by N]");
		std::shared_ptr<math1d_cl::MatData> dMatData;
		std::vector<double> der;
		for (size_t i = 0; i < aData->getNChannelsIdx().size(); i++)
		{
			dMatData.reset(new math1d_cl::MatData(*matData));
			std::shared_ptr<math1d_cl::Channel> dChannel = dMatData->getChannels()[aData->getNChannelsIdx()[i]];
			std::shared_ptr<math1d_cl::Channel> originalChannel = matData->getChannels()[aData->getNChannelsIdx()[i]];
			dChannel->setN(originalChannel->getN() + aData->getP());
	        //m_logger->log("    Channel ID " + std::to_string(originalChannel->getId()) + " N " +  std::to_string(dChannel->getN()));
			
			dMatData->rainfallRunoffModel();
 
			std::vector<double> comp1Q = dMatData->getChannels()[aData->getAmChannelIdx()]->getHydrograph().getQOut();
			std::vector<double> comp2Q = matData->getChannels()[aData->getAmChannelIdx()]->getHydrograph().getQOut();
			std::vector<double> measQ = matData->getMeasuredHydrographsQ()[aData->getAmStationIdx()];
			trim_vectors(comp1Q, measQ);
			trim_vectors(comp2Q, measQ);
			der.push_back((norma(measQ,comp1Q,m_w)-norma(measQ,comp2Q,m_w))/aData->getP());
			//	 der(i) = (norma(measQ,comp1Q,w)-norma(measQ,comp2Q,w))/p;
			dMatData.reset();
		}
		return der;
	
	}

	double Calibration::min_fs(std::shared_ptr<math1d_cl::MatData> matData, std::shared_ptr<ActualData> aData)
	{
		for (size_t i = 0; i < aData->getCnChannelsIdx().size(); i++)
		{
			std::shared_ptr<math1d_cl::Subbasin> currentSubbasin = matData->getSubbasins()[aData->getCnChannelsIdx()[i]];
			double newCnVal = currentSubbasin->getCn() - aData->getStepCn() * aData->getCnDerivation()->at(i);
			// Apply set limits for cn
			newCnVal = keep_limits(newCnVal, aData->getCnLimit());
			currentSubbasin->setCn(newCnVal);
			//std::cout << "New CN value: " << std::to_string(newCnVal)  << "\n";
		}

		for (size_t k = 0; k < aData->getNChannelsIdx().size(); k++)
		{
			std::shared_ptr<math1d_cl::Channel> currentChannel = matData->getChannels()[aData->getNChannelsIdx()[k]];
			double newNVal = currentChannel->getN() - aData->getStepN() * aData->getnDerivation()->at(k);
			// Apply set limits for cn
			newNVal = keep_limits(newNVal, aData->getNLimit());
			currentChannel->setN(newNVal);
			//std::cout << "New N value: " << std::to_string(newNVal) << "\n";


		}

		m_logger->log("[Minimalization run]");
		matData->rainfallRunoffModel();
		std::vector<double> comp2Q = matData->getChannels()[aData->getAmChannelIdx()]->getHydrograph().getQOut();
		std::vector<double> measQ = matData->getMeasuredHydrographsQ()[aData->getAmStationIdx()];
		trim_vectors(comp2Q, measQ);
		return norma(measQ, comp2Q, m_w);
	}

	double Calibration::norma(std::vector<double> Q1, std::vector<double> Q2,double w)
	{
		if(Q1.size() != Q2.size())
		{
			std::cerr << "ERROR: Vectors in norma are not the same size.";
			return 0.0;
		}

		double result = 0.0;
		for(size_t i = 0; i < Q1.size();i++)
		{
			double weight = 1 + w / Q1.size() * (i+1) - w/2;
			double tmp = (Q1[i] - Q2[i])*weight;
			// Sum for euclidean norm
			result += (tmp*tmp);
		
		}
		// TODO: res = res.*(res>0); ???
		double res = sqrt(result);
		return res;
	
	}

	void Calibration::trim_vectors(std::vector<double> &comp2Q, std::vector<double> &measQ)
	{
		if(comp2Q.size() == measQ.size()) return;
		// Trim the vectors of zeroes
		size_t i = 0, lastMeasIdx = 0;
		for(i = 0; i < measQ.size(); i++)
		{
 			if(measQ[i] == 0) 
			{
				measQ.erase(measQ.begin() + i);
			} else {
				lastMeasIdx = i;
			}	
		}
		// Trim the computed vector
		comp2Q.erase(comp2Q.begin() + (lastMeasIdx + 1),comp2Q.end()); 
	}	

	double Calibration::mc_error(std::shared_ptr<math1d_cl::MatData> matData,  std::shared_ptr<math1d_cl::Channel> amChannel, int amStationIdx, double weight)
	{
		std::vector<double> compQ = amChannel->getHydrograph().getQOut();
		std::vector<double> measQ = matData->getMeasuredHydrographsQ()[amStationIdx];
		
		trim_vectors(compQ,measQ); 

		return norma(compQ,measQ, weight);
	}

	std::vector<int> Calibration::upstream_channels(std::vector<std::shared_ptr<math1d_cl::Channel>> matDataChannels, int amChannelIdx)
	{
		// Find upstream channels to current channel
		std::vector<int> upChannels;
		upChannels.push_back(amChannelIdx);
		if(matDataChannels[amChannelIdx]->getUpstreams().size() > 0)
		{
			for (size_t i = 0; i < matDataChannels[amChannelIdx]->getUpstreams().size(); i++)
			{
				std::vector<int> tmp = upstream_channels(matDataChannels, matDataChannels[amChannelIdx]->getUpstreams()[i]);
				upChannels.insert(upChannels.end(), tmp.begin(), tmp.end());
			}
		}
		std::sort(upChannels.begin(),upChannels.end());
		return upChannels;
	}

	double Calibration::keep_limits(double val, Limit limit)
	{
		// If within limits, do nothin'
		if(val >= limit.lower && val <= limit.upper)
		{
			return val;
		} 
		else
		{
			if(val < limit.lower)
			{
				return limit.lower;
			}
			if( val > limit.upper) 
			{
				return limit.upper;
			}
		}

		/* Should never happen. Ever. */
		return val;
	}

	void Calibration::printAll(std::shared_ptr<math1d_cl::MatData> matData)
	{
		for (size_t i = 0; i < matData->getChannels().size(); i++)
		{
			std::cout.precision(5);
			std::cout << "[" << i << "]Channel ID: " << matData->getChannels()[i]->getId() << " N: " << matData->getChannels()[i]->getN() << "\n";
		}
		for (size_t k = 0; k < matData->getChannels().size(); k++)
		{
			std::cout.precision(5);
			std::cout << "[" << k << "]Subbasin ID: " << matData->getSubbasins()[k]->getId() << " CN: " << matData->getSubbasins()[k]->getCn() << "\n";
		}
	}
}