#include "serialcalc.hpp"

#include <vector>
#include "float.h"
#include <iostream>
#include <chrono>



void run_serial(const unsigned char* image, 
        const float* PCA, 
        const float* PCA_mean,
        const float* SVM,
        const float* SVM_intercept,
        const float* WB, //White balance vector
        const float* savgol_coef, //Savitzky-Golay filter
        unsigned char* labels, //Output SVM labels
        const int Nx, //Dimension of the image in x
        const int Ny, //Dimension of the image in y
        const int Nc, //Number of channels in the image
        const int Npca, //Number of PCA components
        const int Nk, //Number of SVM classes
        const int Wl_min, //Minimum wavelength
        const int Wl_max, //Maximum wavelength
        const int savgol, //Toggle Savitzky-Golay filter (0 = off, 1 = on)
        const int white_balance, //Toggle white balance (0 = off, 1 = on)
        const int savgol_window, //Savitzky-Golay window size
        bool print_time = false
        )
        {
            int image_offset = savgol_window / 2;
            auto white_time = std::chrono::duration<double> (0);
            auto sg_time = std::chrono::duration<double> (0);
            auto pca_time = std::chrono::duration<double> (0);
            auto svm_time = std::chrono::duration<double> (0);
            auto overhead_time = std::chrono::duration<double> (0);


            for (int i = 0; i < Ny; i++) {
                for (int j = 0; j < Nx; j++) {

                    auto start = std::chrono::high_resolution_clock::now();
                    std::vector<float> whiteCalibratedSpectrum = std::vector<float>(Nc + 2 * image_offset, 0.0);
                    std::vector<float> sgFilteredSpectrum = std::vector<float>(Nc, 0.0);
                    std::vector<float> pcaScores = std::vector<float>(Npca, 0.0);
                    auto end = std::chrono::high_resolution_clock::now();
                    overhead_time += end - start;

                    
                    // apply white calibration
                    start = std::chrono::high_resolution_clock::now();
                    for (int k = 0; k < Nc; k++) {
                        if (white_balance == 1) {
                            whiteCalibratedSpectrum[k + image_offset] = (float)image[i * Nx * Nc + j * Nc + k] / WB[j * Nc + k] * 255.0f;
                        } else {
                            whiteCalibratedSpectrum[k + image_offset] = (float)image[i * Nx * Nc + j * Nc + k];
                        }
                    }
                    for (int k = 0; k < image_offset; k++) {
                        whiteCalibratedSpectrum[k] = whiteCalibratedSpectrum[image_offset];
                        whiteCalibratedSpectrum[Nc + image_offset + k] = whiteCalibratedSpectrum[Nc + image_offset - 1];
                    }
                    end = std::chrono::high_resolution_clock::now();
                    white_time += end - start;


                    // apply savgol filter
                    start = std::chrono::high_resolution_clock::now();
                    if (savgol == 1) {
                        for (int k = 0; k < Nc; k++) {
                            float sum = 0.0f;
                            for (int l = -savgol_window / 2; l <= savgol_window / 2; l++) {
                                sum += whiteCalibratedSpectrum[k + image_offset + l] * savgol_coef[l + savgol_window / 2];
                            }
                            sgFilteredSpectrum[k] = sum;
                        }
                    } else {
                        for (int k = 0; k < Nc; k++) {
                            sgFilteredSpectrum[k] = whiteCalibratedSpectrum[k + image_offset];
                        }
                    }
                    end = std::chrono::high_resolution_clock::now();
                    sg_time += end - start;


                    // perform PCA
                    start = std::chrono::high_resolution_clock::now();
                    for (int k = 0; k < Npca; k++) {
                        float sum = 0.0f;
                        for (int l = 0; l < Nc; l++) {
                            sum += (sgFilteredSpectrum[l] - PCA_mean[l]) * PCA[k * Nc + l];
                        }
                        pcaScores[k] = sum;
                    }
                    end = std::chrono::high_resolution_clock::now();
                    pca_time += end - start;

                    // perform svm
                    start = std::chrono::high_resolution_clock::now();
                    float svmScore = -FLT_MAX;
                    for (int k = 0; k < Nk; k++) {
                        float sum = 0.0f;
                        for (int l = 0; l < Npca; l++) {
                            sum += pcaScores[l] * SVM[l * Nk + k];
                        }
                        sum += SVM_intercept[k];
                        if (sum > svmScore) {
                            svmScore = sum;
                            labels[i * Nx + j] = k;
                        }
                    }
                    end = std::chrono::high_resolution_clock::now();
                    svm_time += end - start;
                }
            }

            if (print_time) {
                std::cout << "(CPU) White balance time: " << std::chrono::duration_cast<std::chrono::microseconds>(white_time).count() << " mus" << std::endl;
                std::cout << "(CPU) Savitzky-Golay time: " << std::chrono::duration_cast<std::chrono::microseconds>(sg_time).count() << " mus" << std::endl;
                std::cout << "(CPU) PCA time: " << std::chrono::duration_cast<std::chrono::microseconds>(pca_time).count() << " mus" << std::endl;
                std::cout << "(CPU) SVM time: " << std::chrono::duration_cast<std::chrono::microseconds>(svm_time).count() << " mus" << std::endl;
                std::cout << "(CPU) Overhead time: " << std::chrono::duration_cast<std::chrono::microseconds>(overhead_time).count() << " mus" << std::endl;
            }
        }

void run_serial_minimal(const unsigned char* image, 
        const float* PCA, 
        const float* PCA_mean,
        const float* SVM,
        const float* SVM_intercept,
        unsigned char* labels, //Output SVM labels
        const int Nx, //Dimension of the image in x
        const int Ny, //Dimension of the image in y
        const int Nc, //Number of channels in the image
        const int Npca, //Number of PCA components
        const int Nk, //Number of SVM clusters
        const int Wl_min, //Minimum wavelength
        const int Wl_max, //Maximum wavelength
        bool print_time = false
        )
        {
            auto white_time = std::chrono::duration<double> (0);
            auto sg_time = std::chrono::duration<double> (0);
            auto pca_time = std::chrono::duration<double> (0);
            auto svm_time = std::chrono::duration<double> (0);
            auto overhead_time = std::chrono::duration<double> (0);


            for (int i = 0; i < Ny; i++) {
                for (int j = 0; j < Nx; j++) {

                    auto start = std::chrono::high_resolution_clock::now();
                    std::vector<float> pcaScores = std::vector<float>(Npca, 0.0);
                    auto end = std::chrono::high_resolution_clock::now();
                    overhead_time += end - start;

                    
                    // apply white calibration
                    start = std::chrono::high_resolution_clock::now();
                    // for (int k = 0; k < Nc; k++) {
                    //     if (white_balance == 1) {
                    //         whiteCalibratedSpectrum[k + image_offset] = (float)image[i * Nx * Nc + j * Nc + k] / WB[j * Nc + k] * 255.0f;
                    //     } else {
                    //         whiteCalibratedSpectrum[k + image_offset] = (float)image[i * Nx * Nc + j * Nc + k];
                    //     }
                    // }
                    // for (int k = 0; k < image_offset; k++) {
                    //     whiteCalibratedSpectrum[k] = whiteCalibratedSpectrum[image_offset];
                    //     whiteCalibratedSpectrum[Nc + image_offset + k] = whiteCalibratedSpectrum[Nc + image_offset - 1];
                    // }
                    end = std::chrono::high_resolution_clock::now();
                    white_time += end - start;


                    // apply savgol filter
                    start = std::chrono::high_resolution_clock::now();
                    // if (savgol == 1) {
                    //     for (int k = 0; k < Nc; k++) {
                    //         float sum = 0.0f;
                    //         for (int l = -savgol_window / 2; l <= savgol_window / 2; l++) {
                    //             sum += whiteCalibratedSpectrum[k + image_offset + l] * savgol_coef[l + savgol_window / 2];
                    //         }
                    //         sgFilteredSpectrum[k] = sum;
                    //     }
                    // } else {
                    //     for (int k = 0; k < Nc; k++) {
                    //         sgFilteredSpectrum[k] = whiteCalibratedSpectrum[k + image_offset];
                    //     }
                    // }
                    end = std::chrono::high_resolution_clock::now();
                    sg_time += end - start;


                    // perform PCA
                    start = std::chrono::high_resolution_clock::now();
                    for (int k = 0; k < Npca; k++) {
                        float sum = 0.0f;
                        for (int l = 0; l < Nc; l++) {
                            sum += (image[i * Nx * Nc + j * Nc + l] - PCA_mean[l]) * PCA[k * Nc + l];
                        }
                        pcaScores[k] = sum;
                    }
                    end = std::chrono::high_resolution_clock::now();
                    pca_time += end - start;

                    // perform svm
                    start = std::chrono::high_resolution_clock::now();
                    float svmScore = -FLT_MAX;
                    for (int k = 0; k < Nk; k++) {
                        float sum = 0.0f;
                        for (int l = 0; l < Npca; l++) {
                            sum += pcaScores[l] * SVM[l * Nk + k];
                        }
                        sum += SVM_intercept[k];
                        if (sum > svmScore) {
                            svmScore = sum;
                            labels[i * Nx + j] = k;
                        }
                    }
                    end = std::chrono::high_resolution_clock::now();
                    svm_time += end - start;
                }
            }

            if (print_time) {
                std::cout << "(CPU minimal) White balance time: " << std::chrono::duration_cast<std::chrono::microseconds>(white_time).count() << " mus" << std::endl;
                std::cout << "(CPU minimal) Savitzky-Golay time: " << std::chrono::duration_cast<std::chrono::microseconds>(sg_time).count() << " mus" << std::endl;
                std::cout << "(CPU minimal) PCA time: " << std::chrono::duration_cast<std::chrono::microseconds>(pca_time).count() << " mus" << std::endl;
                std::cout << "(CPU minimal) SVM time: " << std::chrono::duration_cast<std::chrono::microseconds>(svm_time).count() << " mus" << std::endl;
                std::cout << "(CPU minimal) Overhead time: " << std::chrono::duration_cast<std::chrono::microseconds>(overhead_time).count() << " mus" << std::endl;
                }
        }