/*
 * AXMIspCorrectState.cpp
 */

#include "AXMCommon/AXMCommonConfig.hpp"
#include "AXMCommon/AXMCommonSetParam.hpp"
#include "AXMCommon/AXMCommonStruct.hpp"

#include "AXMIspCorrect.hpp"
#include "AXMIspCorrectParam.hpp"
#include "AXMIsp/AXMIsp.hpp"
#ifdef BB_TMP_PASS	/* 一旦コンパイルを通す用 */
#include "../AXMIspConfig.hpp"
#else
#include "../../project/refkit/AXMIspConfig.hpp"
#endif

#ifdef USE_AXFTEST_COMMAND
#include "../AXMTest/AXMScenarioTest.hpp"
#include "../AXMIsp/AXMIsp.hpp"
#include "../AXMIspAf/AXMIspAf.hpp"
#include "../AXMIspAe/AXMIspAe.hpp"
#include <stdio.h>
#include <string.h>
#include <sstream>
#include <string>
#endif /* USE_AXFTEST_COMMAND */

#include "AXHSpecParam.hpp"
#include "AXHSpecInit.hpp"

//  補正データ格納先定義
//    シェーディング補正データ/画素欠陥補正アドレスデータ格納先のため、
//    他のオブジェクトからは参照することがない
//    このため、AXHSpec.cppで定義されている変数を外部参照する
extern ST_CORRECT g_Correct;                                //  シェーディング補正データ
extern ST_CDEF    g_Defect_Addr[AXH_Defect_ExtAddr_Max];    //  画素欠陥補正アドレスデータ

// 3rdリリースから、「モデルイベント」と「デバイスイベント」の
// 登録が可能となる。
//
// 状態テーブルは、状態ごとに定義し、イベントIDとメンバ関数の紐付けを行う。
// ※状態テーブルの最後は、必ず{0,NULL}で終了する必要がある。

// STATE_WAITING状態の状態テーブル定義
//      {イベントID, メンバ関数ポインタ}
//      ...
//     {0,NULL}
const AXMIspCorrect::StateTable AXMIspCorrect::state_waiting[] = {
    { AXMIspCorrect::EVENT_SPEC_ADDR,
      (AXMIspCorrect::StateFuncPtr) &AXMIspCorrect::funcSpecAddr }, //  設定値保存先アドレス通知
    { AXMIspCorrect::EVENT_RUNNING,
      (AXMIspCorrect::StateFuncPtr) &AXMIspCorrect::matrix0008   }, //  シェーディング補正モードに移行
    { AXMIspCorrect::EVENT_WAITING,
      (AXMIspCorrect::StateFuncPtr) &AXMIspCorrect::matrix0009   }, //  dummy
    { AXMIspCorrect::EVENT_KIZU_RUNNING,
      (AXMIspCorrect::StateFuncPtr) &AXMIspCorrect::matrix000A   }, //  キズ補正モードに移行

#ifdef USE_AXFTEST_COMMAND
    { AXMIspCorrect::EVENT_AXCOMM_TEST,
      (AXMIspCorrect::StateFuncPtr) &AXMIspCorrect::funcAxcommTest },
    { AXMIspCorrect::EVENT_PRIORITY_TEST,
      (AXMIspCorrect::StateFuncPtr) &AXMIspCorrect::funcPriorityTest },
    { AXMIspCorrect::EVENT_LOOPBACK_TEST,
      (AXMIspCorrect::StateFuncPtr) &AXMIspCorrect::funcLoopbackTest },
#endif /*USE_AXFTEST_COMMAND*/
    {0,NULL}
};

// STATE_RUNNING状態の状態テーブル定義
//      {イベントID, メンバ関数ポインタ}
//      ...
//     {0,NULL}
const AXMIspCorrect::StateTable AXMIspCorrect::state_running[] = {
    { AXMIspCorrect::EVENT_START,
      (AXMIspCorrect::StateFuncPtr) &AXMIspCorrect::matrix0100 }, //  演算開始要求
    { AXMIspCorrect::EVENT_RUNNING,
      (AXMIspCorrect::StateFuncPtr) &AXMIspCorrect::matrix0108 }, //  dummy
    { AXMIspCorrect::EVENT_WAITING,
      (AXMIspCorrect::StateFuncPtr) &AXMIspCorrect::matrix0109 }, //  待機中に移行
    { AXMIspCorrect::EVENT_KIZU_RUNNING,
      (AXMIspCorrect::StateFuncPtr) &AXMIspCorrect::matrix010A }, //  キズ補正モードに移行

#ifdef USE_AXFTEST_COMMAND
    { AXMIspCorrect::EVENT_AXCOMM_TEST,
      (AXMIspCorrect::StateFuncPtr) &AXMIspCorrect::funcAxcommTest },
    { AXMIspCorrect::EVENT_PRIORITY_TEST,
      (AXMIspCorrect::StateFuncPtr) &AXMIspCorrect::funcPriorityTest },
    { AXMIspCorrect::EVENT_LOOPBACK_TEST,
      (AXMIspCorrect::StateFuncPtr) &AXMIspCorrect::funcLoopbackTest },
#endif /*USE_AXFTEST_COMMAND*/
    {0,NULL}
};

// STATE_KIZU_RUNNING状態の状態テーブル定義
//      {イベントID, メンバ関数ポインタ}
//      ...
//     {0,NULL}
const AXMIspCorrect::StateTable AXMIspCorrect::state_kizu_running[] = {
    { AXMIspCorrect::EVENT_KIZU_START,
      (AXMIspCorrect::StateFuncPtr) &AXMIspCorrect::matrix0200 }, //  演算開始要求
    { AXMIspCorrect::EVENT_RUNNING,
      (AXMIspCorrect::StateFuncPtr) &AXMIspCorrect::matrix0208 }, //  シェーディング補正モードに移行
    { AXMIspCorrect::EVENT_WAITING,
      (AXMIspCorrect::StateFuncPtr) &AXMIspCorrect::matrix0209 }, //  待機中に移行
    { AXMIspCorrect::EVENT_KIZU_RUNNING,
      (AXMIspCorrect::StateFuncPtr) &AXMIspCorrect::matrix020A }, //  dummy

#ifdef USE_AXFTEST_COMMAND
    { AXMIspCorrect::EVENT_AXCOMM_TEST,
      (AXMIspCorrect::StateFuncPtr) &AXMIspCorrect::funcAxcommTest },
    { AXMIspCorrect::EVENT_PRIORITY_TEST,
      (AXMIspCorrect::StateFuncPtr) &AXMIspCorrect::funcPriorityTest },
    { AXMIspCorrect::EVENT_LOOPBACK_TEST,
      (AXMIspCorrect::StateFuncPtr) &AXMIspCorrect::funcLoopbackTest },
#endif /*USE_AXFTEST_COMMAND*/
    {0,NULL}
};


ax::actorFuncStatus AXMIspCorrect::funcSpecAddr(const void *pParam, int size) {
  if((0 < size) && (pParam != NULL)) {
    ST_MSG *msg = (ST_MSG *)pParam;
    p_spec = (ST_SPEC *)msg->data;

//  ここでは、次ステータスに進めない
//    state = STATE_RUNNING;
//#ifndef PARASOFT_CPPTEST
//    setNextState(state);
//#endif  //  PARASOFT_CPPTEST
  }
  else {
    m_log.write(AXFLOG_ERR, "Correct funcSpecAddr : Recieve data error");
  }

  return ax::AXFACTOR_SUCCESS;
}

ax::actorFuncStatus AXMIspCorrect::matrix0008(const void *pParam, int size) {
  //  シェーディング補正モードに移行
#ifndef PARASOFT_CPPTEST                //  CPPTESTでは除外
  if (0 > send(nameIsp, AXFEVENT_MODEL(AXMIsp::CORRECTEVENT_SHADE_MODE_CHANGE), 0, 0)) {
    m_log.write(AXFLOG_ERR, "fail send ISP CORRECTEVENT_SHADE_MODE_CHANGE");
  }
#else
  event = AXMIsp::CORRECTEVENT_SHADE_MODE_CHANGE;
#endif  //  PARASOFT_CPPTEST

  state = STATE_RUNNING;                                    //  シェーディング補正モードに移行
#ifndef PARASOFT_CPPTEST
  setNextState(state);
#endif  //  PARASOFT_CPPTEST

  return ax::AXFACTOR_SUCCESS;
}

ax::actorFuncStatus AXMIspCorrect::matrix0009(const void *pParam, int size) {
  //  待機中に移行(終了通知のみ)
#ifndef PARASOFT_CPPTEST                //  CPPTESTでは除外
  if (0 > send(nameIsp, AXFEVENT_MODEL(AXMIsp::CORRECTEVENT_WAIT_MODE_CHANGE), 0, 0)) {
    m_log.write(AXFLOG_ERR, "fail send ISP EVENT_WAIT_MODE_CHANGE");
  }
#else
  event = AXMIsp::CORRECTEVENT_WAIT_MODE_CHANGE;
#endif  //  PARASOFT_CPPTEST

  return ax::AXFACTOR_SUCCESS;
}

ax::actorFuncStatus AXMIspCorrect::matrix000A(const void *pParam, int size) {
  //  キズ補正モードに移行
#ifndef PARASOFT_CPPTEST                //  CPPTESTでは除外
  if (0 > send(nameIsp, AXFEVENT_MODEL(AXMIsp::CORRECTEVENT_KIZU_MODE_CHANGE), 0, 0)) {
    m_log.write(AXFLOG_ERR, "fail send ISP CORRECTEVENT_KIZU_MODE_CHANGE");
  }
#else
  event = AXMIsp::CORRECTEVENT_KIZU_MODE_CHANGE;
#endif  //  PARASOFT_CPPTEST

  state = STATE_KIZU_RUNNING;                               //  キズ補正モードに移行
#ifndef PARASOFT_CPPTEST
  setNextState(state);
#endif  //  PARASOFT_CPPTEST

  return ax::AXFACTOR_SUCCESS;
}

ax::actorFuncStatus AXMIspCorrect::matrix0100(const void *pParam, int size) {
  if((0 < size) && (pParam != NULL)) {
    //  画像データアドレス取得
    ST_MSG *msg = (ST_MSG *)pParam;
    UCHAR *p_img = (UCHAR *)msg->data;
    if(p_spec->LensShade.regCorrectStart == AXH_Shade_Correct_Start) {
      // シェーディング補正開始(1回目)
      //  変数初期化
      cnt = 1;
      //  初回データ取得
      funcIntData(p_img, Shade_IntMode_Set);
      //  シェーディング補正開始リセット
      p_spec->LensShade.regCorrectStart = AXH_Shade_Correct_Reset;
    }
    else if(cnt < Shade_CorrectData_Num) {
      //  シェーディング補正開始(2回目〜[Shade_CorrectData_Num]回目)
      //  平均化のためのデータ取得
      funcIntData(p_img, Shade_IntMode_Add);
      cnt++;
      if(Shade_CorrectData_Num <= cnt) {
        //  補正データ算出
        funcCorrectData();
      }
    }
  }
  else {
    m_log.write(AXFLOG_ERR, "Correct matrix0100 : Recieve data error");
  }

  //  演算実行の有無に関わらず、AXMIspへシェーディング補正演算終了を通知
#ifndef PARASOFT_CPPTEST
  if (0 > send(nameIsp, AXFEVENT_MODEL(AXMIsp::EVENT_SHADE_END), 0, 0)) {
    m_log.write(AXFLOG_ERR, "fail send Isp EVENT_SHADE_END");
  }
#endif  //  PARASOFT_CPPTEST

  return ax::AXFACTOR_SUCCESS;
}

ax::actorFuncStatus AXMIspCorrect::matrix0108(const void *pParam, int size) {
  //  シェーディング補正モードに移行(終了通知のみ)
#ifndef PARASOFT_CPPTEST                //  CPPTESTでは除外
  if (0 > send(nameIsp, AXFEVENT_MODEL(AXMIsp::CORRECTEVENT_SHADE_MODE_CHANGE), 0, 0)) {
    m_log.write(AXFLOG_ERR, "fail send ISP CORRECTEVENT_SHADE_MODE_CHANGE");
  }
#else
  event = AXMIsp::CORRECTEVENT_SHADE_MODE_CHANGE;
#endif  //  PARASOFT_CPPTEST

  return ax::AXFACTOR_SUCCESS;
}

ax::actorFuncStatus AXMIspCorrect::matrix0109(const void *pParam, int size) {
  //  待機中に移行(通知のみ)
#ifndef PARASOFT_CPPTEST                //  CPPTESTでは除外
  if (0 > send(nameIsp, AXFEVENT_MODEL(AXMIsp::CORRECTEVENT_WAIT_MODE_CHANGE), 0, 0)) {
    m_log.write(AXFLOG_ERR, "fail send ISP EVENT_WAIT_MODE_CHANGE");
  }
#else
  event = AXMIsp::CORRECTEVENT_SHADE_MODE_CHANGE;
#endif  //  PARASOFT_CPPTEST

  state = STATE_WAITING;                                    //  シェーディング補正モードに移行
#ifndef PARASOFT_CPPTEST
  setNextState(state);
#endif  //  PARASOFT_CPPTEST

  return ax::AXFACTOR_SUCCESS;
}

ax::actorFuncStatus AXMIspCorrect::matrix010A(const void *pParam, int size) {
  //  キズ補正モードに移行
#ifndef PARASOFT_CPPTEST                //  CPPTESTでは除外
  if (0 > send(nameIsp, AXFEVENT_MODEL(AXMIsp::CORRECTEVENT_KIZU_MODE_CHANGE), 0, 0)) {
    m_log.write(AXFLOG_ERR, "fail send ISP CORRECTEVENT_KIZU_MODE_CHANGE");
  }
#else
  event = AXMIsp::CORRECTEVENT_KIZU_MODE_CHANGE;
#endif  //  PARASOFT_CPPTEST

  state = STATE_KIZU_RUNNING;                               //  キズ補正モードに移行
#ifndef PARASOFT_CPPTEST
  setNextState(state);
#endif  //  PARASOFT_CPPTEST

  return ax::AXFACTOR_SUCCESS;
}

ax::actorFuncStatus AXMIspCorrect::matrix0200(const void *pParam, int size) {
  if((0 < size) && (pParam != NULL)) {
    if(p_spec->Defect.DetectSet.regStart == AXH_Defect_Detect_Start) {
      //  キズ補正開始
      //  画像データアドレス取得
      ST_MSG *msg = (ST_MSG *)pParam;
      UCHAR *p_img = (UCHAR *)msg->data;
      funcDefectData((USHORT *)p_img);
      //  キズ補正開始リセット
      p_spec->Defect.DetectSet.regStart = AXH_Defect_Detect_Reset;
    }
  }
  else {
    m_log.write(AXFLOG_ERR, "Correct matrix0100 : Recieve data error");
  }

  //  演算実行の有無に関わらず、AXMIspへキズ補正演算終了を通知
#ifndef PARASOFT_CPPTEST
  if (0 > send(nameIsp, AXFEVENT_MODEL(AXMIsp::EVENT_KIZU_END), 0, 0)) {
    m_log.write(AXFLOG_ERR, "fail send Isp EVENT_KIZU_END");
  }
#else   //  PARASOFT_CPPTEST
  event = ::AXMIsp::EVENT_KIZU_END;
#endif  //  PARASOFT_CPPTEST

  return ax::AXFACTOR_SUCCESS;
}

ax::actorFuncStatus AXMIspCorrect::matrix0208(const void *pParam, int size) {
  //  シェーディング補正モードに移行
#ifndef PARASOFT_CPPTEST                //  CPPTESTでは除外
  if (0 > send(nameIsp, AXFEVENT_MODEL(AXMIsp::CORRECTEVENT_SHADE_MODE_CHANGE), 0, 0)) {
    m_log.write(AXFLOG_ERR, "fail send ISP CORRECTEVENT_SHADE_MODE_CHANGE");
  }
#else
  event = AXMIsp::CORRECTEVENT_SHADE_MODE_CHANGE;
#endif  //  PARASOFT_CPPTEST

  state = STATE_RUNNING;                                    //  シェーディング補正モードに移行
#ifndef PARASOFT_CPPTEST
  setNextState(state);
#endif  //  PARASOFT_CPPTEST

  return ax::AXFACTOR_SUCCESS;
}

ax::actorFuncStatus AXMIspCorrect::matrix0209(const void *pParam, int size) {
  //  待機中に移行(終了通知のみ)
#ifndef PARASOFT_CPPTEST                //  CPPTESTでは除外
  if (0 > send(nameIsp, AXFEVENT_MODEL(AXMIsp::CORRECTEVENT_WAIT_MODE_CHANGE), 0, 0)) {
    m_log.write(AXFLOG_ERR, "fail send ISP CORRECTEVENT_WAIT_MODE_CHANGE");
  }
#else
  event = AXMIsp::CORRECTEVENT_WAIT_MODE_CHANGE;
#endif  //  PARASOFT_CPPTEST

  state = STATE_WAITING;                                    //  待機中に移行
#ifndef PARASOFT_CPPTEST
  setNextState(state);
#endif  //  PARASOFT_CPPTEST

  return ax::AXFACTOR_SUCCESS;
}

ax::actorFuncStatus AXMIspCorrect::matrix020A(const void *pParam, int size) {
  //  キズ補正モードに移行
#ifndef PARASOFT_CPPTEST                //  CPPTESTでは除外
  if (0 > send(nameIsp, AXFEVENT_MODEL(AXMIsp::CORRECTEVENT_KIZU_MODE_CHANGE), 0, 0)) {
    m_log.write(AXFLOG_ERR, "fail send ISP EVENT_KIZU_MODE_CHANGE");
  }
#else
  event = AXMIsp::CORRECTEVENT_KIZU_MODE_CHANGE;
#endif  //  PARASOFT_CPPTEST

  state = STATE_KIZU_RUNNING;                               //  キズ補正モードに移行
#ifndef PARASOFT_CPPTEST
  setNextState(state);
#endif  //  PARASOFT_CPPTEST

  return ax::AXFACTOR_SUCCESS;
}

void AXMIspCorrect::funcIntData(UCHAR *p_img, Shade_IntMode mode)
{
  UINT width = (UINT)p_spec->Scaler.scaling.InImgSize.Width;
  UINT height = (UINT)p_spec->Scaler.scaling.InImgSize.Height;
  //  水平方向のグリッドのピッチ
  Shade_Grid_Hor_Pitch = width >> SFT_Shade_Grid_Width;
  //  垂直方向のグリッド数
  Shade_Grid_Height = (height >> SFT_Shade_Grid_Ver_Pitch) + 1;
  if((height & MASK_Shade_Grid_Ver_Pitch) != 0x0000000ul) {
    Shade_Grid_Height++;                                    //  余りがあるときは1加算
  }
  //  グリッド0～33行目
  for(UINT i = 0; i < (Shade_Grid_Height - 1); i++) {
    //   グリッド0～31列目
    for(UINT j = 0; j < Shade_Grid_Width; j++) {
      USHORT *pStartData;
      if(j < (Shade_Grid_Width - 1)) {
        pStartData = (USHORT*)(p_img + ((i * Shade_Grid_Ver_Pitch) * width + j * Shade_Grid_Hor_Pitch) * Shade_Pixcel_Data_Size);
      }
      else {
        pStartData = (USHORT*)(p_img + ((i * Shade_Grid_Ver_Pitch) * width + j * Shade_Grid_Hor_Pitch - 2) * Shade_Pixcel_Data_Size);
      }
      //  14Bit/画素 データ時処理
      USHORT *pData_r = (USHORT *)(pStartData);
      USHORT *pData_b = (USHORT *)(pStartData + width);
      UINT rData  = *pData_r++;
      UINT grData = *pData_r;
      UINT gbData = *pData_b++;
      UINT bData  = *pData_b;
      if(mode == Shade_IntMode_Set) {
        IntData[i][j].R = rData;
        IntData[i][j].G = ((grData + gbData) >> 1);
        IntData[i][j].B = bData;
      }
      else {
        IntData[i][j].R += rData;
        IntData[i][j].G += ((grData + gbData) >> 1);
        IntData[i][j].B += bData;
      }
    }
  }
  {
    //  グリッド34行目(33行目のコピー)
    UINT i = Shade_Grid_Height - 1;
    for(UINT j = 0; j < Shade_Grid_Width; j++) {
      IntData[i][j].R = IntData[i-1][j].R;
      IntData[i][j].G = IntData[i-1][j].G;
      IntData[i][j].B = IntData[i-1][j].B;
    }
  }
}

void AXMIspCorrect::funcCorrectData() {
  UINT sft = p_spec->LensShade.Shift;
  UINT coef = 64 * (1 << sft) / Shade_Grid_Ver_Pitch;
  UINT cpnt_w = Shade_Grid_Width >> 1;
  UINT cpnt_h = Shade_Grid_Height >> 1;
  //  補正データ算出
  for(UINT i = 0; i < Shade_Grid_Height; i++) {
    for(UINT j = 0; j < Shade_Grid_Width; j++) {
      if((IntData[i][j].R != 0) && (IntData[i][j].G != 0) && (IntData[i][j].B != 0)) {
        //  どれか一つでも分母が0であれば、演算しない
        g_Correct.Col[i][j].R  = coef * IntData[cpnt_h][cpnt_w].R / IntData[i][j].R;
        g_Correct.Col[i][j].Gr = coef * IntData[cpnt_h][cpnt_w].G / IntData[i][j].G;
        g_Correct.Col[i][j].Gb = g_Correct.Col[i][j].Gr;
        g_Correct.Col[i][j].B  = coef * IntData[cpnt_h][cpnt_w].B / IntData[i][j].B;
      }
      else {
        g_Correct.Data[i][j] = 0x00000000ul;
      }
    }
  }
  //  補正データ平均
  for(UINT i = 0; i < cpnt_h; i++) {
    for(UINT j = 0; j < cpnt_w; j++) {
      UINT sumR = 0, sumG = 0, sumB = 0;
      UINT cnt = 0;
      //  左上加算
      sumR += g_Correct.Col[cpnt_h - i][cpnt_w - j].R;
      sumG += g_Correct.Col[cpnt_h - i][cpnt_w - j].Gr;
      sumB += g_Correct.Col[cpnt_h - i][cpnt_w - j].B;
      cnt++;
      //  右上加算
      if((cpnt_w + j) < AXH_Shade_Grid_Width) {
        sumR += g_Correct.Col[cpnt_h - i][cpnt_w + j].R;
        sumG += g_Correct.Col[cpnt_h - i][cpnt_w + j].Gr;
        sumB += g_Correct.Col[cpnt_h - i][cpnt_w + j].B;
        cnt++;
      }
      //  右下加算
      if(((cpnt_h + i) < Shade_Grid_Height) && ((cpnt_w + j) < AXH_Shade_Grid_Width)) {
        sumR += g_Correct.Col[cpnt_h + i][cpnt_w + j].R;
        sumG += g_Correct.Col[cpnt_h + i][cpnt_w + j].Gr;
        sumB += g_Correct.Col[cpnt_h + i][cpnt_w + j].B;
        cnt++;
      }
      //  左下加算
      if((cpnt_h + i) < Shade_Grid_Height) {
        sumR += g_Correct.Col[cpnt_h + i][cpnt_w - j].R;
        sumG += g_Correct.Col[cpnt_h + i][cpnt_w - j].Gr;
        sumB += g_Correct.Col[cpnt_h + i][cpnt_w - j].B;
        cnt++;
      }

      if(0 < cnt) {
        //  平均算出
        UINT aveR = sumR / cnt;
        UINT aveG = sumG / cnt;
        UINT aveB = sumB / cnt;
        //  左上設定
        g_Correct.Col[cpnt_h - i][cpnt_w - j].R  = aveR;
        g_Correct.Col[cpnt_h - i][cpnt_w - j].Gr = aveG;
        g_Correct.Col[cpnt_h - i][cpnt_w - j].Gb = aveG;
        g_Correct.Col[cpnt_h - i][cpnt_w - j].B  = aveB;
        //  右上設定
        if((cpnt_w + j) < AXH_Shade_Grid_Width) {
          g_Correct.Col[cpnt_h - i][cpnt_w + j].R  = aveR;
          g_Correct.Col[cpnt_h - i][cpnt_w + j].Gr = aveG;
          g_Correct.Col[cpnt_h - i][cpnt_w + j].Gb = aveG;
          g_Correct.Col[cpnt_h - i][cpnt_w + j].B  = aveB;
        }
        //  右下設定
        if(((cpnt_h + i) < Shade_Grid_Height) && ((cpnt_w + j) < AXH_Shade_Grid_Width)) {
          g_Correct.Col[cpnt_h + i][cpnt_w + j].R  = aveR;
          g_Correct.Col[cpnt_h + i][cpnt_w + j].Gr = aveG;
          g_Correct.Col[cpnt_h + i][cpnt_w + j].Gb = aveG;
          g_Correct.Col[cpnt_h + i][cpnt_w + j].B  = aveB;
        }
        //  左下設定
        if((cpnt_h + i) < Shade_Grid_Height) {
          g_Correct.Col[cpnt_h + i][cpnt_w - j].R  = aveR;
          g_Correct.Col[cpnt_h + i][cpnt_w - j].Gr = aveG;
          g_Correct.Col[cpnt_h + i][cpnt_w - j].Gb = aveG;
          g_Correct.Col[cpnt_h + i][cpnt_w - j].B  = aveB;
        }
      }
    }
  }
}

void AXMIspCorrect::funcDefectData(USHORT *p_Img) {
  INT pntCnt = 0;
  USHORT width = p_spec->Scaler.scaling.InImgSize.Width;
  USHORT height = p_spec->Scaler.scaling.InImgSize.Height;

  //  補正アドレス初期化
  INT pnt_Max = initDefectData();

  //  欠陥画素検出
  for(USHORT i = 0; i < height; i++) {
    for(UINT j = 0; j < width; j++) {
//        if((*p_Img++ > p_spec->Defect.DetectSet.TH) && (pntCnt < pntCnt_Max)) {
      if(*p_Img > p_spec->Defect.DetectSet.TH) {
        //  欠陥画素検出時:データチェインの先頭に画素情報を追加する
        if(DefectPixcel == NULL) {
          DefectPixcel = new st_defect_pixcel;
          DefectPixcel->p_next = NULL;
        }
        else {
          st_defect_pixcel *defect_tmp = DefectPixcel;
          DefectPixcel = new st_defect_pixcel;
          DefectPixcel->p_next = defect_tmp;
        }
        //  データ登録
        DefectPixcel->H = j;                                //  水平方向座標
        DefectPixcel->V = i;                                //  垂直方向座標
        DefectPixcel->Bright = *p_Img;                      //  輝度
        DefectPixcel->fl_Serial = false;                    //  連欠陥判定フラグ
        pntCnt++;
      }
      p_Img++;
#ifdef  ENABLE_DEFECT_CORRECT_LIMITED
      if(pntCnt_Max <= pntCnt) {
        //  設定上限(＋α)を超えれば、検出終了とする
     	break;
      }
#endif  //  ENABLE_DEFECT_CORRECT_LIMITED
    }
  }
  if(pnt_Max < pntCnt) {                //  (最大記録件数:内部16点/外部64点) < (実検出件数)
    //  補正優先順位設定により、画素設定を行う
    if(p_spec->Defect.DetectSet.Priority == 0) {            //  輝度レベル順
      funcSortDefectData_PriorityBright();
    }
    else {                                                  //  連欠陥優先
      //  連欠陥順に設定
      funcSortDefectData_PrioritySerial();
    }
  }
  //  アドレス指定モード(内部/外部)の設定内容に従って画素情報を記録する
  setDefectData();
}

void AXMIspCorrect::funcSortDefectData_PriorityBright() {
  //  補正優先順位：輝度レベル順
  st_defect_pixcel *p_DefectPixcel = NULL;
  st_defect_pixcel *p_defect_st = DefectPixcel;
  //  最大輝度の画素が先頭になるように並び替え
  while(p_defect_st != NULL) {
    USHORT BrightMax = p_defect_st->Bright;
    st_defect_pixcel *p_defect = p_defect_st->p_next;
    st_defect_pixcel *p_defect_prev = p_defect_st;
    while(p_defect != NULL) {
      if(BrightMax < p_defect->Bright) {                    //  最大輝度検出
        BrightMax = p_defect->Bright;
        //  先頭データを退避
        st_defect_pixcel *p_defect_tmp1 = p_defect_st;
        //  最大データを検索開始位置に移動
        p_defect_st = p_defect;
        //  次回検索データを退避
        st_defect_pixcel *p_defect_tmp2 = p_defect_st->p_next;
        //  検索開始位置データを最大データの次につなぐ
        p_defect_st->p_next = p_defect_tmp1;
        //  次回検索データを2個前のデータにつなぐ
        p_defect_prev->p_next = p_defect_tmp2;
      }
      if(p_defect->p_next != NULL) {
        //  次回検索データセット
        p_defect_prev = p_defect;
        p_defect = p_defect->p_next;
      }
      else {
        //  次回検索データがないので、breakする
        break;
      }
    }
    if(p_DefectPixcel == NULL) {
      //  並び替え後の先頭位置を記録(初回検索の最大値)
      p_DefectPixcel = p_defect_st;
    }
    if(p_defect_st->p_next != NULL) {
      //  次回検索開始データセット
      p_defect_st = p_defect_st->p_next;
    }
    else {
      //  次回検索データがないので、breakする
      break;
    }
  }
  DefectPixcel = p_DefectPixcel;
}

void AXMIspCorrect::funcSortDefectData_PrioritySerial() {
  //  補正優先順位：連欠陥順
  st_defect_pixcel *p_DefectPixcel = NULL;
  st_defect_pixcel *p_defect_st = DefectPixcel;
  //  連欠陥(上下左右に連続する画素)があれば、先頭から順に並ぶように並び替え
  while(p_defect_st != NULL) {
    st_defect_pixcel *p_defect = p_defect_st->p_next;       //  チェック対象
    //  連欠陥判定
    while(p_defect != NULL) {
      if(((p_defect->H == (p_defect_st->H + 1)) && (p_defect->V == p_defect_st->V)) ||  //  基準画素から右の画素
         ((p_defect->H == (p_defect_st->H - 1)) && (p_defect->V == p_defect_st->V)) ||  //  基準画素から左の画素
         ((p_defect->H == p_defect_st->H) && (p_defect->V == (p_defect_st->V - 1))) ||  //  基準画素から上の画素
         ((p_defect->H == p_defect_st->H) && (p_defect->V == (p_defect_st->V + 1)))) {  //  基準画素から下の画素
        p_defect_st->fl_Serial = true;
        p_defect->fl_Serial = true;
      }
      if(p_defect->p_next != NULL) {
        //  次回検索データセット
        p_defect = p_defect->p_next;
      }
      else {
        //  次回検索データがないので、breakする
        break;
      }
    }
    if(p_defect_st->p_next != NULL) {
      //  次回検索開始データセット
      p_defect_st = p_defect_st->p_next;                //  基準データの次のデータを繋ぐ
    }
    else {
      //  次回検索データがないので、breakする
      break;
    }
  }
  //  連欠陥順に並び替え
  p_DefectPixcel = NULL;
  p_defect_st = DefectPixcel;
  st_defect_pixcel *p_DefectPixcel_ptr = NULL;
  st_defect_pixcel *p_DefectPixcelNotSerial = NULL;
  st_defect_pixcel *p_DefectPixcelNotSerial_ptr = NULL;
  while(p_defect_st != NULL) {
  	if(p_defect_st->fl_Serial == true ) {
  	  //  連欠陥のデータチェインに接続する
  	  if(p_DefectPixcel == NULL) {
  	    p_DefectPixcel = p_defect_st;                       //  連欠陥の先頭を登録する
  	    p_DefectPixcel_ptr = p_defect_st;                   //  連欠陥の接続先を登録する
  	  }
  	  else {
  	    p_DefectPixcel_ptr->p_next = p_defect_st;           //  連欠陥のデータチェインに連欠陥データを接続
  	    p_DefectPixcel_ptr = p_DefectPixcel_ptr->p_next;    //  連欠陥の接続先を登録する
  	  }
  	}
  	else {
  	  //  連欠陥でないデータチェインに接続する
      if(p_DefectPixcelNotSerial == NULL) {
         p_DefectPixcelNotSerial = p_defect_st;             //  連欠陥の先頭を登録する
    	  p_DefectPixcelNotSerial_ptr = p_defect_st;         //  連欠陥の接続先を登録する
      }
      else {
        p_DefectPixcelNotSerial_ptr->p_next = p_defect_st;                      //  連欠陥のデータチェインに連欠陥データを接続
        p_DefectPixcelNotSerial_ptr = p_DefectPixcelNotSerial_ptr->p_next;      //  連欠陥の接続先を登録する
      }
  	}
    if(p_defect_st->p_next != NULL) {
      //  次回検索開始データセット
      p_defect_st = p_defect_st->p_next;
    }
    else {
//      //  連欠陥のデータの最後に連欠陥でないデータを接続する
//      p_DefectPixcel_ptr->p_next = p_DefectPixcelNotSerial;
//      p_DefectPixcelNotSerial_ptr->p_next = NULL;
      //  次回検索データがないので、breakする
      break;
    }
  }
//  DefectPixcel = p_DefectPixcel;
  if(p_DefectPixcel != NULL) {
    //  連欠陥のデータがあるとき
    //    欠陥補正データの先頭に連欠陥データを接続する
    DefectPixcel = p_DefectPixcel;
    if(p_DefectPixcelNotSerial != NULL) {
      //  連欠陥でないデータがあるとき
      //    連欠陥データの最後尾に連欠陥でないデータを接続する
      p_DefectPixcel_ptr->p_next = p_DefectPixcelNotSerial;
      //    連欠陥でないデータの最後尾にNULLをセットする
      p_DefectPixcelNotSerial_ptr->p_next = NULL;
    }
    else {
      //  連欠陥でないデータがないとき
      //    連欠陥データの最後尾にNULLをセットする
      p_DefectPixcel_ptr->p_next = NULL;
    }
  }
  else {
    //  連欠陥のデータがないとき
    if(p_DefectPixcelNotSerial != NULL) {
      //  連欠陥でないデータがあるとき
      //    欠陥補正データの先頭に連欠陥でないデータを接続する
      DefectPixcel = p_DefectPixcelNotSerial;
      //    欠陥補正データの最後尾にNULLをセットする
      p_DefectPixcelNotSerial_ptr->p_next = NULL;
    }
  }
}

INT AXMIspCorrect::initDefectData() {
  INT pnt_Max = 0;

  if(p_spec->Defect.Mode.External == AXH_Defect_Mode_Internal) {
    //  アドレス指定モード設定 内部選択時
//    pntCnt_Max = AXH_Defect_IntAddr_Max;
    pnt_Max = AXH_Defect_IntAddr_Max;
    for(UINT i = 0; i < AXH_Defect_IntAddr_Max; i++) {
      p_spec->Defect.Address[i].H = AXH_DEFECT_CDEF_H_Init;
      p_spec->Defect.Address[i].V = AXH_DEFECT_CDEF_V_Init;
      p_spec->Defect.Address[i].CLS = AXH_DEFECT_CDEF_CLS_Init;
    }
  }
  else {
    //  アドレス指定モード設定 外部選択時
//    pntCnt_Max = AXH_Defect_ExtAddr_Max;
    pnt_Max = AXH_Defect_ExtAddr_Max;
    for(UINT i = 0; i < AXH_Defect_ExtAddr_Max; i++) {
      g_Defect_Addr[i].CDEF_H = AXH_DEFECT_CDEF_H_Init;
      g_Defect_Addr[i].CDEF_V = AXH_DEFECT_CDEF_V_Init;
      g_Defect_Addr[i].CDEF_CLS = AXH_DEFECT_CDEF_CLS_Init;
    }
  }
  return pnt_Max;
}

void AXMIspCorrect::setDefectData() {
  if(p_spec->Defect.Mode.External == AXH_Defect_Mode_Internal) {
    //  アドレス指定モード:内部
    setDefectData_Internal();
  }
  else {
    //  アドレス指定モード:外部
    setDefectData_External();
  }
}

void AXMIspCorrect::setDefectData_Internal() {
  UINT cnt = 0;

  //  データチェインに繋がれたデータは全て削除する
  while(DefectPixcel != NULL) {
    st_defect_pixcel *defect_tmp = DefectPixcel;
    if(cnt < AXH_Defect_IntAddr_Max) {
      p_spec->Defect.Address[cnt].H   = defect_tmp->H;
      p_spec->Defect.Address[cnt].V   = defect_tmp->V;
      p_spec->Defect.Address[cnt].CLS = p_spec->Defect.DetectSet.CLS;
      cnt++;
    }
    DefectPixcel = defect_tmp->p_next;
    delete defect_tmp;
  }
}

void AXMIspCorrect::setDefectData_External() {
  UINT cnt = 0;

  //  データチェインに繋がれたデータは全て削除する
  while(DefectPixcel != NULL) {
    st_defect_pixcel *defect_tmp = DefectPixcel;
    if(cnt < AXH_Defect_ExtAddr_Max) {
      g_Defect_Addr[cnt].CDEF_H = defect_tmp->H;
      g_Defect_Addr[cnt].CDEF_V = defect_tmp->V;
      g_Defect_Addr[cnt].CDEF_CLS = p_spec->Defect.DetectSet.CLS;
      cnt++;
    }
    DefectPixcel = defect_tmp->p_next;
    delete defect_tmp;
  }
}

#ifdef USE_AXFTEST_COMMAND
ax::actorFuncStatus AXMIspCorrect::funcAxcommTest(const void *pParam, int size) {
  m_log.write(AXFLOG_SCENARIO, "@Correct funcAxcommTest()");
  m_log.write(AXFLOG_SCENARIO, (char *) pParam);

  return ax::AXFACTOR_SUCCESS;
}

ax::actorFuncStatus AXMIspCorrect::funcPriorityTest(const void *pParam, int size) {
  m_log.write(AXFLOG_SCENARIO, "Correct PriorityTest");
#ifndef PARASOFT_CPPTEST
  pthread_pri_dbgprint();
#endif  //  PARASOFT_CPPTEST

  //受信データからカウンタ値を取得
  int count = 0;
  if(pParam != NULL){
    count = *((int*)pParam);
  }

  //Ispへ結果送信
  int stat = send(nameIsp, AXMIsp::EVENT_PRISCHED_END, (void *)&count, sizeof(count));
  if (0 > stat) {
    m_log.write(AXFLOG_ERR, "fail send Isp Correct Prisched End");
  }
  return ax::AXFACTOR_SUCCESS;
}

ax::actorFuncStatus AXMIspCorrect::funcMemTest(const void *pParam, int size) {
  int addr, bytes, i, *p, pat, v;
  unsigned char *mp;
  char msg[100];

  addr = 0x20000000 + 2 * 1024 * 1024;
  //addr = 0x20000000 + 2 * 1024 * 1024 + 511 * 1024;
  mp = (unsigned char *) addr;
  bytes = 3 * 1024 * 1024;
  //bytes = 3 * 1024 * 1024 - 511 * 1024;

  pat = 0;
  p = (int *) mp;
  for (i = 0; i < bytes / 4; i++)
    *p++ = pat++;

  pat = 0;
  p = (int *) mp;
  for (i = 0; i < bytes / 4; i++) {
    v = *p++;
    if (v != pat) {
      sprintf(msg, "!!! %08x %08x!=%08x", addr + i * 4, v, pat);
      m_log.write(AXFLOG_INFO, msg);
    }
    pat++;
  }

  return ax::AXFACTOR_SUCCESS;
}

ax::actorFuncStatus AXMIspCorrect::funcLoopbackTest(const void *pParam, int size) {
  m_log.write(AXFLOG_SCENARIO, "@Correct funcLoopbackTest()");

  std::ostringstream loopCountStr;
  loopCountStr << m_loopbackCount;
  m_log.write(
      AXFLOG_SCENARIO,
      "loopcount=" + loopCountStr.str() + " rcvdata=" + (char*)(pParam));
  m_loopbackCount++;

  AXFObjectName axtestName("axtest", "axStage");
  char buf[30];
  memcpy(buf, (char*)pParam, size);
  int i;
  for (i = 0; i < size; i += 2)
    buf[i] += 0x20;
  // EVENT_LOOPBACK_TEST
  if (0 > send(axtestName, AXFEVENT_MODEL(2), buf, size)) {
    m_log.write(AXFLOG_ERR, "fail send loopback");
  }

  return ax::AXFACTOR_SUCCESS;
}
#endif /* USE_AXFTEST_COMMAND */

