// Class for analyzing the user's face expression

#include "../const.h"

#ifdef AIBO_YES

#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <string.h>
#include "FaceExpr.h"
#include "Image.h"
#include "ProcessFace.h"
#include "BMP.h"
#include "PrintResults.h"
#include "ExpandLayer.h"
#include "MapOnExpanded.h"
#include "Blocks.h"
#include "ProcessExpression.h"
#include "BlockMatching.h"
#include "AU.h"
#include "Emotion.h"
#include "../common/include/MoNetData.h"


#include <OPENR/OSyslog.h>
#include <OPENR/ODataFormats.h>
#include <OPENR/OFbkImage.h>
#include <OPENR/OPENRAPI.h>
#include <OPENR/core_macro.h>


FaceExpr::FaceExpr(): faceExprState(IDLE),
                      fbkID(oprimitiveID_UNDEF)
{
}

OStatus
FaceExpr::DoInit(const OSystemEvent& event)
{
    NEW_ALL_SUBJECT_AND_OBSERVER;
    REGISTER_ALL_ENTRY;
    SET_ALL_READY_AND_NOTIFY_ENTRY;

    // from "ImageObserver.cc" sample

    char design[orobotdesignNAME_MAX+1];
    OStatus result = OPENR::GetRobotDesign(design);
    if (result != oSUCCESS) {
        OSYSLOG1((osyslogERROR, "%s : %s %d",
                  "ImageObserver::DoInit()",
                  "OPENR::GetRobotDesign() FAILED", result));
    }
    if (!strcmp(design, "ERS-210") || !strcmp(design, "ERS-220")) {
        robot = ROBOT_ERS200;
    } else if (!strcmp(design, "ERS-7")) {
        robot = ROBOT_ERS7;
    } else {
        robot = ROBOT_UNDEF;
        OSYSLOG1((osyslogERROR,
                  "ImageObserver::DoInit() UNKNOWN ROBOT DESIGN"));
    }

    // Initializing sensors...
    OpenPrimitive();

    // Setting camera...
    SetCameraParameter();

    // Initializing parameters...
    #ifndef ONLINE
    ib = Init_batch();
    #endif

    parameters = Init_para();

    // Setting CDT channel...
    SetChannel();

    return oSUCCESS;
}


OStatus
FaceExpr::DoStart(const OSystemEvent& event)
{
    ENABLE_ALL_SUBJECT;
    ASSERT_READY_TO_ALL_OBSERVER;
    return oSUCCESS;
}

OStatus
FaceExpr::DoStop(const OSystemEvent& event)
{
    faceExprState = IDLE;
    DISABLE_ALL_SUBJECT;
    DEASSERT_READY_TO_ALL_OBSERVER;
    return oSUCCESS;
}

OStatus
FaceExpr::DoDestroy(const OSystemEvent& event)
{
    DELETE_ALL_SUBJECT_AND_OBSERVER;
    return oSUCCESS;
}

void
FaceExpr::ProcessPicture(const ONotifyEvent& event)
{
    bool result;

    #ifdef TIME_REC
    SystemTime startTime;
    SystemTime endTime;
    SystemTime elapsed;
    #endif

    static int counter = 0;
    // adapted from "ImageObserver.cc" sample

    if (faceExprState == IDLE) {
        observer[event.ObsIndex()]->AssertReady(event.SenderID());
        return; // do nothing
    }

    if (faceExprState == START) {
        // sound message
        PRN(("Make a neutral face and touch my back sensor.\n"));
        
        ExecuteMoNetCommand(INTRO_NEUTRAL_WAV);
        faceExprState = INTRO_NEUTRAL;

        observer[event.ObsIndex()]->AssertReady(event.SenderID());
        return;
    }
    
    if (faceExprState == INTRO_NEUTRAL) {
        observer[event.ObsIndex()]->AssertReady(event.SenderID());
        return; // do nothing
    }

    OFbkImageVectorData* fbkImageVectorData  = (OFbkImageVectorData*)event.Data(0);

    if (robot == ROBOT_ERS7) {
           if (faceExprState == PROCESSING_NEUTRAL) {
              OSensorValue val1, valf, valm, valr;
              OPENR::GetSensorValue(tinswID,  &val1);
              OPENR::GetSensorValue(backf7ID, &valf);
              OPENR::GetSensorValue(backm7ID, &valm);
              OPENR::GetSensorValue(backr7ID, &valr);
              
              // Ending interaction
              if (val1.value == oswitchON) {
                  PRN(("Goodbye! Interaction ending...\n"));
                  
                  ExecuteMoNetCommand(ENDING);
                  faceExprState = WAITING_FOR_END;
                  
                  observer[event.ObsIndex()]->AssertReady(event.SenderID());
                  return;
                                          	
              }

              else if (valf.value >= 12 || valm.value >= 14 || valr.value >= 14) {

                         PRN(("Starting...\n"));
                         while (ExistData(counter) == true) counter++;
                         //SaveData(counter, fbkImageVectorData);

                         #ifdef TIME_REC
                               GetSystemTime(&startTime);
                         #endif

                          // Initialization: setting pointers to NULL value
                          face_centroid = face_bb = rec_aus = NULL;
                          blocks = NULL;
                          
                          motion = false;

                         // Reading Cr image
                         im_cr = Image(fbkImageVectorData, ofbkimageLAYER_M, ofbkimageBAND_Cr);
                         // Reading Cb image
                         im_cb = Image(fbkImageVectorData, ofbkimageLAYER_M, ofbkimageBAND_Cb);
                         // Reading clustered image
                         im_clust = Image(fbkImageVectorData, ofbkimageLAYER_C, ofbkimageBAND_CDT);
                         // Reading expanded image
                         im_expanded = Image(fbkImageVectorData);

                         // Processing Face...
                         result = ProcessFace(counter, im_cr, im_cb, im_clust, parameters, 
                                              mouth, eye_left, eye_right, face_bb, face_centroid);
                         if (!result) {
                            PRN(("Could not correctly detect your face\n"));
                            faceExprState = ALL_DONE;
                         }
                         else {
                                                          // Mapping found features on the expanded image
                            result = ExpandImages(mouth, eye_left, eye_right, mouth_exp, eye_left_exp,
                                                  eye_right_exp, face_bb);
                            if (!result) {
                                PRN(("Mapping on expanded image failed.\n"));
                                faceExprState = ALL_DONE;
                            }
                            else {
                                                                             // Getting Eyebrows
                                  result = GetBothEyebrows(eye_left_exp, eye_right_exp, face_bb, im_expanded, 
                                                           eyebrow_left, eyebrow_right, parameters, counter);
                                  if (!result) {
                                      PRN(("Could not correctly detect your face\n"));
                                      faceExprState = ALL_DONE;
                                  }
                                  else {
                                                                                  // Showing results
                                        if (parameters.debug_face)
                                           PrintResults(mouth_exp, eye_left_exp, eye_right_exp, eyebrow_left,
                                                        eyebrow_right, im_expanded, counter);
                    
                                                                                  // Placing blocks
                                         PlaceBlocks(im_expanded, mouth_exp, eyebrow_left, eyebrow_right, face_bb, 
                                                     blocks, parameters, counter);

                                         if (!result) {
                                            PRN(("Could not correctly detect your face\n"));
                                            faceExprState = ALL_DONE;
                                         }
                                         else  {
                                            faceExprState = NEUTRAL_DONE;
                                            PRN(("Face done.\n"));
                                            
                                            #ifdef TIME_REC
                                                 GetSystemTime(&endTime);
                                                 elapsed = (endTime - startTime);
                                                 PRN(("TIME: %u sec, %u microsec\n", elapsed.seconds, elapsed.useconds));
                                            #endif
                                        }
                                   }
                              }
                         }
                }
            }
            else if (faceExprState == NEUTRAL_DONE) {
                    // sound message
                    PRN(("Make your expression and touch my back sensor.\n"));

                    ExecuteMoNetCommand(INTRO_EXPRESSION_WAV);
                    faceExprState = INTRO_EXPR;

                    observer[event.ObsIndex()]->AssertReady(event.SenderID());

                    return;
            }
            
            else if (faceExprState == INTRO_EXPR) {
                    observer[event.ObsIndex()]->AssertReady(event.SenderID());
                    return; // do nothing
            }

            else if (faceExprState == PROCESSING_EXPR) {

                          OSensorValue valf, valm, valr;
                          OPENR::GetSensorValue(backf7ID, &valf);
                          OPENR::GetSensorValue(backm7ID, &valm);
                          OPENR::GetSensorValue(backr7ID, &valr);

                          if (valf.value >= 12 || valm.value >= 14 || valr.value >= 14) {

                                PRN(("Starting...\n"));


                                #ifdef TIME_REC
                                     GetSystemTime(&startTime);
                                #endif
      
                                                                       // Reading expression image...
                                im_expr_clust = Image(fbkImageVectorData, ofbkimageLAYER_C, ofbkimageBAND_CDT);
                                im_expr_cb    = Image(fbkImageVectorData, ofbkimageLAYER_M, ofbkimageBAND_Cb);
                                im_expr_cr    = Image(fbkImageVectorData, ofbkimageLAYER_M, ofbkimageBAND_Cr);
                                im_expression = Image(fbkImageVectorData);
      
                                                                       // Processing Expression
                                result = ProcessExpression(im_expanded, im_expression, im_expr_clust, face_centroid,
                                                           face_bb, im_expr_cr, eye_left, eye_right, im_cr, blocks, 
                                                           eyes_status, parameters, counter);
                                if (!result) {
                                   PRN(("Could not process expression.\n"));
                                   emotion = Emotion(0);
                                   faceExprState = EXPR_DONE;
                                }
                                else {
                                                                    // AU recognition
                                   result = RecognizeAU(blocks, eyes_status, face_bb, rec_aus, parameters);
                                   if (!result) {
                                      PRN(("Could not recognize Action Units.\n"));
                                      emotion = Emotion(0);
                                      faceExprState = EXPR_DONE;
                                   }
                                   else {
                                                                   // Emotion recognition
                                      result = RecognizeEmotion(rec_aus, emotion, parameters);
                                      if (!result) {
                                         PRN(("Could not recognize emotions.\n"));
                                         emotion = Emotion(0);
                                         faceExprState = EXPR_DONE;
                                      }
      
                                      #ifdef TIME_REC
                                               GetSystemTime(&endTime);
                                               elapsed = (endTime - startTime);
                                               PRN(("TIME: %u sec, %u microsec\n", elapsed.seconds, elapsed.useconds));
                                      #endif
      
                                      faceExprState = EXPR_DONE;
                                      PRN(("All done.\n"));
                                    }
                               }
                               // Showing recognized emotion via LEDs...
                               subject[sbjShowLED]->SetData(&(emotion.name), sizeof(emotion.name));
                               subject[sbjShowLED]->NotifyObservers();


                           }
              }
              else if (faceExprState == EXPR_DONE) {

                          motion = true;

                          // Interacting...
                          subject[sbjInteract]->SetData(&emotion, sizeof(emotion));
                          subject[sbjInteract]->NotifyObservers();

                          faceExprState = ALL_DONE;

              }
              else if (faceExprState == ALL_DONE) {

                          // Freeing memory....
                          FreeImg(mouth);
                          FreeImg(eye_left);
                          FreeImg(eye_right);
                          FreeImg(mouth_exp);
                          FreeImg(eye_left_exp);
                          FreeImg(eye_right_exp);
                          FreeImg(eyebrow_left);
                          FreeImg(eyebrow_right);
                          FreeImg(im_cr);
                          FreeImg(im_cb);
                          FreeImg(im_clust);
                          FreeImg(im_expanded);
                          FreeImg(im_expression);
                          FreeImg(im_expr_clust);
                          FreeImg(im_expr_cb);
                          FreeImg(im_expr_cr);
                          FreeEmotion(emotion);
                          free(face_centroid);
                          free(face_bb);
                          free(blocks);
                          free(rec_aus);


                          PRN(("Memory freed.\n"));
                          
                          if (motion)
                             faceExprState = IDLE;
                          else
                             faceExprState = START;

              }
              
              else if (faceExprState == WAITING_FOR_END) {
                    observer[event.ObsIndex()]->AssertReady(event.SenderID());
                    return; // do nothing
              }


        }

    observer[event.ObsIndex()]->AssertReady(event.SenderID());
    return;
}


void
FaceExpr::ExecuteMoNetCommand(int command)
{

#ifdef FUNC_TRACK
PRN(("Starting ExecuteMoNetCommand\n"));
#endif

MoNetCommand cmd(command);
subject[sbjPlay]->SetData(&cmd, sizeof(cmd));
subject[sbjPlay]->NotifyObservers();

#ifdef FUNC_TRACK
PRN(("Ending ExecuteMoNetCommand\n"));
#endif

return;

}

void
FaceExpr::NotifyMoNetResult(const ONotifyEvent& event)
{

#ifdef FUNC_TRACK
PRN(("Starting NotifySoundResult\n"));
#endif

MoNetResult* result = (MoNetResult*)event.Data(0);

if (result->status == monetCOMPLETION) {
    if (faceExprState == INTRO_NEUTRAL)
       faceExprState = PROCESSING_NEUTRAL;
    else if (faceExprState == INTRO_EXPR)
       faceExprState = PROCESSING_EXPR;
    else if (faceExprState == WAITING_FOR_END)
       faceExprState = IDLE;
    else if (faceExprState == IDLE)
       faceExprState = START;
}

observer[event.ObsIndex()]->AssertReady();


#ifdef FUNC_TRACK
PRN(("Ending NotifySoundResult\n"));
#endif

return;

}


// From "ImageObserver.cc" sample
void
FaceExpr::OpenPrimitive()
{
    OStatus result = OPENR::OpenPrimitive(FBK_LOCATOR, &fbkID);
    if (result != oSUCCESS) {
        OSYSLOG1((osyslogERROR, "%s : %s %d",
                  "ImageObserver::OpenPrimitive()",
                  "OPENR::OpenPrimitive() FAILED", result));
    }

    result = OPENR::OpenPrimitive(TINSW_LOCATOR, &tinswID);
    if (result != oSUCCESS) {
        OSYSLOG1((osyslogERROR, "%s : %s %d",
                  "ImageObserver::OpenPrimitive()",
                  "OPENR::OpenPrimitive() FAILED", result));
    }

    if (robot == ROBOT_ERS200) {
        result = OPENR::OpenPrimitive(BACKSW_LOCATOR, &backswID);
        if (result != oSUCCESS) {
            OSYSLOG1((osyslogERROR, "%s : %s %d",
                      "ImageObserver::OpenPrimitive()",
                      "OPENR::OpenPrimitive() FAILED", result));
        }
    } else if (robot == ROBOT_ERS7) {
        result = OPENR::OpenPrimitive(BACKF7_LOCATOR, &backf7ID);
        if (result != oSUCCESS) {
            OSYSLOG1((osyslogERROR, "%s : %s %d",
                      "ImageObserver::OpenPrimitive()",
                      "OPENR::OpenPrimitive() FAILED", result));
        }
        result = OPENR::OpenPrimitive(BACKM7_LOCATOR, &backm7ID);
        if (result != oSUCCESS) {
            OSYSLOG1((osyslogERROR, "%s : %s %d",
                      "ImageObserver::OpenPrimitive()",
                      "OPENR::OpenPrimitive() FAILED", result));
        }
        result = OPENR::OpenPrimitive(BACKR7_LOCATOR, &backr7ID);
        if (result != oSUCCESS) {
            OSYSLOG1((osyslogERROR, "%s : %s %d",
                      "ImageObserver::OpenPrimitive()",
                      "OPENR::OpenPrimitive() FAILED", result));
        }
    }
}

// From "ImageObserver.cc" sample
bool
FaceExpr::ExistData(int serialNumber)
{
    char name[128];
    struct stat st;

    sprintf(name, "%sIMCB%04d.BMP", DATA_FOLDER, serialNumber);
    if (stat(name, &st) == 0) return true;
    
    sprintf(name, "%sIMCR%04d.BMP", DATA_FOLDER, serialNumber);
    if (stat(name, &st) == 0) return true;

    sprintf(name, "%sCOL%04d%d.BMP", DATA_FOLDER, serialNumber, 0);
    if (stat(name, &st) == 0) return true;
    
    sprintf(name, "%sL_HR%04d.BMP", DATA_FOLDER, serialNumber);
    if (stat(name, &st) == 0) return true;

    return false;
}


// From "ImageObserver.cc" sample
void
FaceExpr::SaveData(int serialNumber, OFbkImageVectorData* imageVec)
{
    char name[128];
    BMP bmp;
    byte* image;
    int width, height;


    sprintf(name, "%sIMCB%04d.BMP", DATA_FOLDER, serialNumber);
    OSYSPRINT(("writing %s ...\n", name));
    bmp.SaveCbBand(name, imageVec, ofbkimageLAYER_M);


    sprintf(name, "%sIMCR%04d.BMP", DATA_FOLDER, serialNumber);
    OSYSPRINT(("writing %s ...\n", name));
    bmp.SaveCrBand(name, imageVec, ofbkimageLAYER_M);


    // Saving expanded H layer...
    ReconstructImage(imageVec, ofbkimageLAYER_H, &image, &width, &height);
    sprintf(name, "%sL_HR%04d.BMP", DATA_FOLDER, serialNumber);
    OSYSPRINT(("writing %s ...\n", name));
    bmp.SaveRaw2Gray(name, image, width, height, 0);
    FreeImage(image);

    // Clustering image
    sprintf(name, "%sCOL%04d", DATA_FOLDER, serialNumber);
    OSYSPRINT(("writing %s ...\n", name));
    bmp.SaveLayerC(name, imageVec, 1);


    OSYSPRINT(("Everything done.\n"));

}


void
FaceExpr::SetChannel()
{
    OStatus result;
    MemoryRegionID cdtVecID;
    OCdtVectorData* cdtVec;
    OCdtInfo* cdt;

    result = OPENR::NewCdtVectorData(&cdtVecID, &cdtVec);
    if (result != oSUCCESS){
        OSYSLOG1((osyslogERROR, "%s : %s %d",
                  "Prova::SetChannel()",
                  "OPENR::NewCdtVectorData() FAILED", result));
        return;
    }


    cdtVec->SetNumData(1);
    cdt = cdtVec->GetInfo(0);
    cdt->Init(fbkID, 1);



    // skin (Y, Cr, Cb)
    for (int i = 0; i < 32; i++) {
        if (i >= (parameters.y_min/8))
           cdt->Set(i, parameters.cr_max, parameters.cr_min, parameters.cb_max, parameters.cb_min); 
        else
           cdt->Set(i, 0, 0, 0, 0);
    }

    result = OPENR::SetCdtVectorData(cdtVecID);
    if (result != oSUCCESS)  {
        OSYSLOG1((osyslogERROR, "%s : %s %d",
                  "Prova::SetChannel()",
                  "OPENR::SetCdtVectorData() FAILED", result));
    }


    result = OPENR::DeleteCdtVectorData(cdtVecID);
    if (result != oSUCCESS) {
        OSYSLOG1((osyslogERROR, "%s : %s %d",
                  "Prova::SetChannel()",
                  "OPENR::DeleteCdtVectorData() FAILED", result));
    }

}

// From "ImageObserver.cc" sample
bool
FaceExpr::ReconstructImage(OFbkImageVectorData* imageVec,
                                OFbkImageLayer layer,
                                byte** image, int* width, int* height)
{
    *image = 0;
    *width = *height = 0;
    if (layer == ofbkimageLAYER_C) return false;

    OFbkImageInfo* info = imageVec->GetInfo(layer);
    byte*          data = imageVec->GetData(layer);

    OFbkImage yLLImg(info, data, ofbkimageBAND_Y); // Y_LL
    OFbkImage yLHImg(info, data, ofbkimageBAND_Y_LH);
    OFbkImage yHLImg(info, data, ofbkimageBAND_Y_HL);
    OFbkImage yHHImg(info, data, ofbkimageBAND_Y_HH);

    int w = yLLImg.Width();
    int h = yLLImg.Height();

    byte* img = (byte*)malloc((2*w)*(2*h));
    if (img == 0) return false;

    for (int y = 0; y < h; y++) {

        for (int x = 0; x < w; x++) {
            //
            // yLH, yHL, yHH : offset binary [0, 255] -> signed int [-128, 127]
            //
            int yLL = (int)yLLImg.Pixel(x, y);
            int yLH = (int)yLHImg.Pixel(x, y) - 128;
            int yHL = (int)yHLImg.Pixel(x, y) - 128;
            int yHH = (int)yHHImg.Pixel(x, y) - 128;

            int a = yLL + yLH + yHL + yHH; // ypix11
            int b = 2 * (yLL + yLH);       // ypix11 + ypix01
            int c = 2 * (yLL + yHL);       // ypix11 + ypix10
            int d = 2 * (yLL + yHH);       // ypix11 + ypix00
            
            byte ypix00 = ClipRange(d - a);
            byte ypix10 = ClipRange(c - a);
            byte ypix01 = ClipRange(b - a);
            byte ypix11 = ClipRange(a);
            
            PutPixel(img, 2*w, 2*x,   2*y,   ypix00);
            PutPixel(img, 2*w, 2*x+1, 2*y,   ypix10);
            PutPixel(img, 2*w, 2*x,   2*y+1, ypix01);
            PutPixel(img, 2*w, 2*x+1, 2*y+1, ypix11);
        }

    }

    *image  = img;
    *width  = 2 * w;
    *height = 2 * h;

    return true;
}


// Laura's
bool
FaceExpr::ReconstructImageColor(OFbkImageVectorData* imageVec,
                                OFbkImageLayer layer,
                                byte** image, int* width, int* height)
{
    *image = 0;
    *width = *height = 0;
    if (layer == ofbkimageLAYER_C) return false;

    OFbkImageInfo* info = imageVec->GetInfo(layer);
    byte*          data = imageVec->GetData(layer);

    OFbkImage yLLImg(info, data, ofbkimageBAND_Y); // Y_LL
    OFbkImage yLHImg(info, data, ofbkimageBAND_Y_LH);
    OFbkImage yHLImg(info, data, ofbkimageBAND_Y_HL);
    OFbkImage yHHImg(info, data, ofbkimageBAND_Y_HH);
    
    OFbkImage crImg(info, data, ofbkimageBAND_Cr);
    OFbkImage cbImg(info, data, ofbkimageBAND_Cb);
    
    byte pixels[3];

    int w = yLLImg.Width();
    int h = yLLImg.Height();

    byte* img = (byte*)malloc((2*w)*(2*h));
    if (img == 0) return false;

    for (int y = 0; y < h; y++) {

        for (int x = 0; x < w; x++) {
            //
            // yLH, yHL, yHH : offset binary [0, 255] -> signed int [-128, 127]
            //
            int yLL = (int)yLLImg.Pixel(x, y);
            int yLH = (int)yLHImg.Pixel(x, y) - 128;
            int yHL = (int)yHLImg.Pixel(x, y) - 128;
            int yHH = (int)yHHImg.Pixel(x, y) - 128;

            int a = yLL + yLH + yHL + yHH; // ypix11
            int b = 2 * (yLL + yLH);       // ypix11 + ypix01
            int c = 2 * (yLL + yHL);       // ypix11 + ypix10
            int d = 2 * (yLL + yHH);       // ypix11 + ypix00
            
            byte ypix00 = ClipRange(d - a);
            byte ypix10 = ClipRange(c - a);
            byte ypix01 = ClipRange(b - a);
            byte ypix11 = ClipRange(a);
            
            byte cb = cbImg.Pixel(x, y);
	    byte cr = crImg.Pixel(x, y);

	    pixels[0]= ypix00;
	    pixels[1]= cb;
	    pixels[2]= cr;

	    PutPixel(img, 2*w, 2*x,   2*y,   pixels);
	    
	    pixels[0] = ypix10;
            PutPixel(img, 2*w, 2*x+1, 2*y,   pixels);
            
            pixels[0] = ypix01;
            PutPixel(img, 2*w, 2*x,   2*y+1, pixels);
            
            pixels[0] = ypix11;
            PutPixel(img, 2*w, 2*x+1, 2*y+1, pixels);

            

        }

    }

    *image  = img;
    *width  = 2 * w;
    *height = 2 * h;

    return true;
}




// From "ImageObserver.cc" sample
void
FaceExpr::FreeImage(byte* image)
{
    if (image != 0) free(image);
}

// Gilberto's
void
FaceExpr::SetCameraParameter()
{
  
  OPrimitiveControl_CameraParam shutter(ocamparamSHUTTER_MID);
  OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_SHUTTER_SPEED, &shutter, sizeof(shutter ) , 0 , 0);
  
  OPrimitiveControl_CameraParam gain(ocamparamGAIN_HIGH);
  OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_GAIN, &gain , sizeof(gain), 0,0);
  
  OPrimitiveControl_CameraParam wb(ocamparamWB_INDOOR_MODE) ;
  OPENR::ControlPrimitive (fbkID,oprmreqCAM_SET_WHITE_BALANCE, &wb, sizeof(wb),0,0);

  OPENR::ControlPrimitive (fbkID, oprmreqCAM_AWB_ON, 0, 0, 0, 0);
  OPENR::ControlPrimitive (fbkID, oprmreqCAM_AE_ON, 0, 0, 0, 0);
  
}


#endif






