// Spice_SOM_For_FaceVideo.cpp
// g++ Spice_SOM_For_FaceVideo.cpp.cpp mlpsom_source/spice_som_lib.cpp -lm -O3 -DUSE_OPENCV -I/usr/local/include -I.build_release/src -I./src -I./include -L/usr/lib -L/usr/local/lib -L/usr/lib  -lopencv_core -lopencv_highgui -lopencv_imgproc -lopencv_imgcodecs  -ffast-math -fopenmp -o som_output.x

#include <iostream>
#include <fstream>
#include <stdio.h>
#include <time.h>
#include <sys/timeb.h>
#include <math.h>
#include <vector>
#include <omp.h>
#include <float.h>
#include "opencv2/core/core.hpp"
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "mlpsom_include/spice_som_lib.h"
#include <sys/time.h> 
#include <unistd.h>

using namespace std;
using namespace cv;
using namespace SpiceNeuro;

Size bsize = Size(32,45);  // 40,54 size for face


struct str_filename
{
   char fname[255];
};
double GetDblTimeNow()
{
 struct timeval t;
 gettimeofday(&t, 0);
 return ((double)t.tv_sec + ((double)t.tv_usec*1.0e-6));

  struct timeb tmb;
  double sec;
  double milisec;
  ftime(&tmb);
  sec     = (double) tmb.time;
  milisec = (double) tmb.millitm;
  sec += milisec/1000.;
  return sec;
};
// create an image of a neuron weight
// chanel = 1 or 3
Mat neuronImage(float *weight, int chanel)
{
   if ((chanel != 1)&&(chanel != 3)){
      printf("unknown chanel\n");
      exit(0);
   }   
   Mat box = Mat(bsize.height, bsize.width, CV_8UC1, Scalar(128));
   // find the max and min in the weight
   float max = -100000000000000, min = -max;
   int i, j;
   for (j = 0; j < bsize.height; j++){
      for (i = 0; i < bsize.width; i++){
         if (max < weight[j*bsize.width + i]) max = weight[j*bsize.width + i];
         if (min > weight[j*bsize.width + i]) min = weight[j*bsize.width + i];
      }
   }
   int ind = 0;
   for (j = 0; j < bsize.height; j++) {
      for (i = 0; i < bsize.width; i++) {
       for (int c = 0; c < chanel; c++) // 3 chanels
		 {
            float val =(weight[ind++]-min)/(max-min);
            if (val < 0) val = 0;
            if (val > 1.0) val = 1.0;
            uchar uval = (uchar)(val*255);

            if (chanel == 1)
               ((uchar *)(box.data + (j)*box.step))[i] = uval;
            else 
               ((uchar *)(box.data + (j)*box.step))[3*i+c] = uval; // 3 chanels
            
         }
      }
   }
   return box;
}

// weight_image = 1: create an image in that each neuron is average of its weights
// weight_image = 0: create an image in that each neuron is average of samples on it
Mat som_map_image(SpiceS *som, int weight_image, int chanel)
{
   // start the som
   const int nxNodes = som->GetCols();
   const int nyNodes = som->GetRows();
   //const int iDataLength = bsize.width * bsize.height*3;
   //printf("bsize.width=%i, bsize.height = %i\n", bsize.width, bsize.height);
   Mat som_img;
   if (chanel == 3) som_img = Mat(bsize.height*nyNodes, bsize.width*nxNodes, CV_8UC3, Scalar(0, 0, 0));
   else som_img = Mat(bsize.height*nyNodes, bsize.width*nxNodes, CV_8UC1, Scalar(128));

   for (int j = 0; j < nyNodes; j++) {
      for (int i = 0; i < nxNodes; i++){
         int pos = j*nxNodes + i;
         Mat box ;//= neuronImage(som->Nodes[pos]->weights, 1);
         if (!weight_image) 
            box = neuronImage(som->SumOfSamples[pos]->weights, chanel); // using sum of sample as image
            
         else {
            //box = neuronImage(som->Nodes[pos]->weights, chanel); // using weights
            if ((int)som->Nodes[pos]->winners_remake.size() > 0){
               // using one sample on winner_remake
               int sample_index = (int)som->Nodes[pos]->winners_remake[0];
               //box = neuronImage(som->TrainingData->fTrainingSetIn[sample_index], chanel); // using som data
               box = neuronImage(som->TrainingImage->fTrainingSetIn[sample_index], chanel); // using som original image data
            }
            else{
               if (chanel == 3) box = Mat(bsize.height, bsize.width, CV_8UC3, Scalar(0,0,0));
               else box = Mat(bsize.height, bsize.width, CV_8UC1, Scalar(128));
            }
         }
         
         
         //printf("Mat img ok, j=%i, i = %i, pos = %i\n", j, i, pos);
         //printf("Mat box with = %i, Mat box height = %i\n", box.cols, box.rows);
         box.copyTo(som_img(cv::Rect(i*bsize.width,j*bsize.height,bsize.width, bsize.height)));
      }
   }
   printf("Created som_img ok, nx=%i, ny = %i\n", nxNodes, nyNodes);
   printf("       som_img with = %i, som_img height = %i\n", som_img.cols, som_img.rows);

   return som_img;
}

// return an image and assign to SOM data, from data_pos position
int readOneImageBoxes(char *imgfolder, char *imgname, S_Data *TrainingData, int data_pos)
{
   char fname[255];
   sprintf(fname, "%s/%s", imgfolder, imgname);
   Mat src64 = imread(fname, 0); // read image as bw
   if (!src64.data)
   {
      printf("!src64.data\n");
      return 0;
   }
   Mat src32;
   resize (src64, src32, bsize);//resize image as bsize, defined on top of this file

   // check the data
   if (src32.data)
   {
       if (src32.cols%bsize.width != 0)  printf("%s: src32.cols[%i]/ bsize.width[%i] != 0\n", fname, src32.cols, bsize.width);
       if (src32.rows%bsize.height != 0) printf("%s: src32.rows[%i]/ bsize.heigh[%i] != 0\n", fname, src32.rows, bsize.height);
   }
   if (!src32.data)
   {
      printf("!src32.data is failed \n");
      return 0;
   }

   //assign pixel value to the som data som->TrainingData[b]
   int ind = 0;
   for (int j = 0; j < bsize.height; j++)
   {
      for (int i = 0; i < bsize.width; i++)
      {
         TrainingData->fTrainingSetIn[data_pos][ind++]= (float)(((uchar *)(src32.data + (j)*src32.step))[i] - 128)/255.0;
      }
   }

   return 0;
}
// read a roi of an image and assign to SOM data, from data_pos position
// bw image only
int readOneROIImageBoxes(char *imgfolder, char *imgname, S_Data *TrainingData, int data_pos, cv::Rect roi)
{
   char fname[255];
   sprintf(fname, "%s/%s", imgfolder, imgname);
   Mat src64 = imread(fname, 0); // read image as bw
   if (!src64.data)
   {
      printf("!src64.data\n");
      return 0;
   }

   Mat mat_roi = src64(roi).clone();
   Mat src32;
   resize (src64, src32, bsize);//resize image as bsize, defined on top of this file

   // check the data
   if (src32.data)
   {
       if (src32.cols%bsize.width != 0)  printf("%s: src32.cols[%i]/ bsize.width[%i] != 0\n", fname, src32.cols, bsize.width);
       if (src32.rows%bsize.height != 0) printf("%s: src32.rows[%i]/ bsize.heigh[%i] != 0\n", fname, src32.rows, bsize.height);
   }
   if (!src32.data)
   {
      printf("!src32.data is failed \n");
      return 0;
   }

   //assign pixel value to the som data som->TrainingData[b]
   int ind = 0;
   for (int j = 0; j < bsize.height; j++)
   {
      for (int i = 0; i < bsize.width; i++)
      {
          // convert black to white
         int u = (((uchar *)(src32.data + (j)*src32.step))[i]); //0~255
         int ub = 255-u;
         if (ub < 0) ub = 0;
         if (ub > 255) ub = 255;
          
         //TrainingData->fTrainingSetIn[data_pos][ind++]= (float)(((uchar *)(src32.data + (j)*src32.step))[i] - 128)/255.0;
         TrainingData->fTrainingSetIn[data_pos][ind++]= (float)(ub - 128)/255.0;
      }
   }

   return 0;
}

int main_faces_som_outputs(int argc, char **argv)
{

   double startReadDataTime = GetDblTimeNow();

   // face image
   char facefolder[]    = "Data/all_att_faces_jpg";
   char faceindex[]     = "Data/all_att_faces_jpg/index.txt";
   
   char nonfacefolder[] =  "Data/human_faces_negative_cao_made";
   char nonfaceindex[]  =  "Data/human_faces_negative_cao_made/index.txt";
   
   // original image
   char original_facefolder[]    = "Data/all_att_faces_jpg";
   char original_nonfacefolder[] =  "Data/human_faces_negative_cao_made";
  
   char imgname[255];
   vector <struct str_filename> FACEFILENAME;
   vector <struct str_filename> NONFACEFILENAME;
   FILE *fface = fopen(faceindex, "r");
   FILE *fnonface = fopen(nonfaceindex, "r");
   
   struct str_filename strfilename;

   int positivesample = 0;
   while (fscanf(fface, "%s", imgname) > 0)
   {
      if (strcmp(imgname, "finish") == 0) break;
      if (positivesample >= 700) break;
      positivesample++;
      sprintf(strfilename.fname, "%s", imgname);
      FACEFILENAME.push_back(strfilename);
   };
   fclose(fface);
   int negativesample = 0;
   while (fscanf(fnonface, "%s", imgname) > 0)
   {
      if (strcmp(imgname, "finish") == 0) break;
      if (negativesample >= 700) break; // use 700 negative image only
      negativesample ++;
      sprintf(strfilename.fname, "%s", imgname);
      NONFACEFILENAME.push_back(strfilename);
   };
   fclose(fnonface);
   
   int nFaceFiles    = (int)FACEFILENAME.size();
   int nNonFaceFiles = (int)NONFACEFILENAME.size();
   // end of read image index

   // start the som
   // faces
   //const int nxNodes = 60;
   //const int nyNodes = 25;
   // pedestrian
   //const int nxNodes = 85;
   //const int nyNodes = 22;
   
  
   const int iDataLength = bsize.width * bsize.height;//*3 if 3 chanels;
   SpiceS *som = new SpiceS(nxNodes, nyNodes, iDataLength, nFaceFiles + nNonFaceFiles);
   // init training image (original image, for creating output map
   som->TrainingImage = new S_Data(nFaceFiles + nNonFaceFiles, iDataLength);
   printf("new som ()ok\n");
   
   char somfilename[255], somimagefilename[255];
   sprintf(somfilename, "SOM_For_FACES_nx=%i_ny=%i.dat", som->GetCols(), som->GetRows());

   som->ResetRandomWeight();
   printf("ResetRandomWeight()ok\n");
   // load the som from 2nd train
   //som->loadWeight(somfilename);
   
   // load image data
   for (int data_pos = 0; data_pos < nFaceFiles; data_pos++){
      readOneImageBoxes(facefolder, FACEFILENAME[data_pos].fname, som->TrainingData, data_pos);
      readOneImageBoxes(original_facefolder, FACEFILENAME[data_pos].fname, som->TrainingImage, data_pos);
   }
   for (int data_pos = 0; data_pos < nNonFaceFiles; data_pos++){
      readOneImageBoxes(nonfacefolder, NONFACEFILENAME[data_pos].fname, som->TrainingData, nFaceFiles + data_pos);
      readOneImageBoxes(original_nonfacefolder, NONFACEFILENAME[data_pos].fname, som->TrainingImage, nFaceFiles + data_pos);
   }
   printf("FACEFILENAME.size    = %i\n", nFaceFiles);
   printf("NONFACEFILENAME.size = %i\n", nNonFaceFiles);
   printf("nFiles               = %i\n", nFaceFiles + nNonFaceFiles);
   printf("read data time       = %f\n============\n", GetDblTimeNow() - startReadDataTime);


   int iCPU = omp_get_num_procs();
   printf("Number of CPU = %i\n", iCPU);
   if (iCPU > 2)
      iCPU --;
   if (som->GetNNodes() < iCPU)
      iCPU = som->GetNNodes();

   // training the som 
   som->ShufflingData();
   printf("som->ShufflingData() ok\n");
   som->SetRandomSampleForWeight();;
   printf("som->SetRandomSampleForWeight() ok\n");
   float sum_sse = 0.0f;

   float lstart = 0.001;
   float lmin   = 0.000001;
   float sstart = max(som->GetCols(), som->GetRows());
   float smin = 4;
   som->param.fSigmaNow = sstart;
   som->param.fLearningRateNow = lstart;
   som->param.bShowTrainingProgress = true;
   
   char sname[255];
   Mat som_img_weight = som_map_image(som, 1, 1);
   sprintf(somimagefilename, "output_tmp/SOM_For_FACE_nx=%i_ny=%i.dat_files_loop_-1_weight.jpg", som->GetCols(), som->GetRows());
   imwrite(somimagefilename, som_img_weight);
   Mat som_img_sample = som_map_image(som, 0, 1);
   sprintf(somimagefilename, "output_tmp/SOM_For_FACE_nx=%i_ny=%i.dat_files_loop_-1_sample.jpg", som->GetCols(), som->GetRows());
   imwrite(somimagefilename, som_img_sample);
         
   for (int loop = 0; loop < 500; loop++)
   {
      printf("\tIteration %i\n", loop);
      // assign random value to inputOrder, can be ignore in case of increasing learning speed
      // return number of boxes on the image
      double t1=0, t2=0, t3=0;

      // adjust param after each iteration
      //som->param.fSigmaNow = som->param.fSigmaNow -1;
      //if (som->param.fSigmaNow <2 ) som->param.fSigmaNow = 2;
      //som->param.fLearningRateNow = som->param.fLearningRateNow * 0.95;
      //if (som->param.fLearningRateNow < 0.0001) som->param.fLearningRateNow = 0.0001;


      // adjust param each 100 files, because there are lost of files
      if (loop > 0 && loop % 2 == 0)
      {
         som->param.fSigmaNow = som->param.fSigmaNow -1;
         if (som->param.fSigmaNow <smin ) som->param.fSigmaNow = smin;
         som->param.fLearningRateNow = som->param.fLearningRateNow * 0.95;
         if (som->param.fLearningRateNow < lmin) som->param.fLearningRateNow = lmin;
      }

      t1 = GetDblTimeNow();
      float sum_sse = som->trainingOneIteration(0, som->TrainingData->nDataSets, som->output, NF_Linear);
      double sumtraintime = GetDblTimeNow()-t1;

      printf("\titeration %i sum_sse = %f sumtraintime %f s  sigma = %.2f, lrate = %.8f, maxminsse=%f/%f\n", 
            loop, sum_sse, sumtraintime,  som->param.fSigmaNow, som->param.fLearningRateNow, som->maxsse, som->minsse);

      //if (loop > 0 && loop % 10 == 0) // save after each 10 iteration
      {
         printf("loop=%i save som at %s\n", loop, somfilename);
         //som->saveWeight(somfilename);

         //save again, just in case
         //sprintf(sname, "SOM_For_FACE_nx=%i_ny=%i_at_loop=%i.dat", som->GetCols(), som->GetRows(), loop);
         //som->saveWeight(sname);
         Mat som_img_weight = som_map_image(som, 1, 1);
         sprintf(somimagefilename, "output_tmp/SOM_For_FACE_nx=%i_ny=%i.dat_files_loop_%i_weight.jpg", som->GetCols(), som->GetRows(), loop);
         imwrite(somimagefilename, som_img_weight);
         //Mat som_img_sample = som_map_image(som, 0, 1);
         //sprintf(somimagefilename, "output_tmp/SOM_For_FACE_nx=%i_ny=%i.dat_files_loop_%i_sample.jpg", som->GetCols(), som->GetRows(), loop);
         //imwrite(somimagefilename, som_img_sample);
      }
   }
   // end of training the som

   //som->saveWeight(somfilename);
   delete som->TrainingImage;
   delete som;

   return 0;
}