//
// Copyright 2003 Sony Corporation
//
// Permission to use, copy, modify, and redistribute this software for
// non-commercial use is hereby granted.
//
// This software is provided "as is" without warranty of any kind,
// either expressed or implied, including but not limited to the
// implied warranties of fitness for a particular purpose.
//


// Adapted from original "BlinkingFaceLED" Sony sample


#include "../const.h"

#ifdef AIBO_YES

#include <OPENR/OPENRAPI.h>
#include <OPENR/OSyslog.h>
#include <OPENR/core_macro.h>
#include "BlinkingFaceLED.h"

BlinkingFaceLED::BlinkingFaceLED() : blinkingLEDState(BFLS_IDLE)
{
    for (int i = 0; i < NUM_LEDS; i++) ledID[i] = oprimitiveID_UNDEF;
    for (int i = 0; i < NUM_COMMAND_VECTOR; i++) region[i] = 0;
}

OStatus
BlinkingFaceLED::DoInit(const OSystemEvent& event)
{
    OSYSDEBUG(("BlinkingFaceLED::DoInit()\n"));

    NEW_ALL_SUBJECT_AND_OBSERVER;
    REGISTER_ALL_ENTRY;
    SET_ALL_READY_AND_NOTIFY_ENTRY;

    OpenPrimitives();
    NewCommandVectorData();
    OStatus st = OPENR::SetMotorPower(opowerON);
    OSYSDEBUG(("OPENR::SetMotorPower(opowerON) %d\n", st));
    
    return oSUCCESS;
}

OStatus
BlinkingFaceLED::DoStart(const OSystemEvent& event)
{
    OSYSDEBUG(("BlinkingFaceLED::DoStart()\n"));

    blinkingLEDState = BFLS_IDLE;

    ENABLE_ALL_SUBJECT;
    ASSERT_READY_TO_ALL_OBSERVER;

    return oSUCCESS;
}

OStatus
BlinkingFaceLED::DoStop(const OSystemEvent& event)
{
    OSYSDEBUG(("BlinkingFaceLED::DoStop()\n"));

    blinkingLEDState = BFLS_IDLE;

    DISABLE_ALL_SUBJECT;    
    DEASSERT_READY_TO_ALL_OBSERVER;

    return oSUCCESS;
}

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

void
BlinkingFaceLED::StartLED(const ONotifyEvent& event)
{

    int emotion = *((int*)event.Data(0));

    if (blinkingLEDState == BFLS_IDLE) {
        if (subject[sbjBlink]->IsReady() == true) {
            switch(emotion) {
                case 0: blinkingLEDState = BFLS_NEUTRAL;
                        break;
                case 1: blinkingLEDState = BFLS_JOYFUL;
                        break;
                case 2: blinkingLEDState = BFLS_SAD;
                        break;
                case 3: blinkingLEDState = BFLS_SURPRISED;
                        break;
                case 4: blinkingLEDState = BFLS_ANGRY;
                        break;
                case 5: blinkingLEDState = BFLS_FEARFUL;
                        break;
                case 6: blinkingLEDState = BFLS_DISGUSTED;
                        break;
                default: break;

            }
            ModeA_0();
        }

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


}

void
BlinkingFaceLED::Ready(const OReadyEvent& event)
{
    OSYSDEBUG(("BlinkingFaceLED::Ready()\n"));


    if (blinkingLEDState == BFLS_IDLE) {

        OSYSDEBUG(("BFLS_IDLE\n"));
        ; // do nothing

    }

     if (blinkingLEDState == BFLS_NEUTRAL) {    // User Neutral
  
          LED3Result r = Mode_Neutral();
          if (r != LED3_FINISH) {
            blinkingLEDState = BFLS_NEUTRAL;
          }
          else {
            blinkingLEDState = BFLS_IDLE;
            ModeA_0();
         }
      }

      if (blinkingLEDState == BFLS_JOYFUL) {    // User Joyful
  
          LED3Result r = Mode_Joyful();
          if (r != LED3_FINISH) {
            blinkingLEDState = BFLS_JOYFUL;
          }
          else {
            blinkingLEDState = BFLS_IDLE;
            ModeA_0();
         }
      }

      else if (blinkingLEDState == BFLS_SAD) {      // User Sad
  
          LED3Result r = Mode_Sad();
          if (r != LED3_FINISH) {
            blinkingLEDState = BFLS_SAD;
          }
          else {
            blinkingLEDState = BFLS_IDLE;
            ModeA_0();
         }


      }
      else if (blinkingLEDState == BFLS_SURPRISED) {      // User Surprised
  
          LED3Result r = Mode_Surprised();
          if (r != LED3_FINISH) {
            blinkingLEDState = BFLS_SURPRISED;
          }
          else {
            blinkingLEDState = BFLS_IDLE;
            ModeA_0();
         }

  
      }
      else if (blinkingLEDState == BFLS_ANGRY) {      // User Angry
  
          LED3Result r = Mode_Angry();
          if (r != LED3_FINISH) {
            blinkingLEDState = BFLS_ANGRY;
          }
          else {
            blinkingLEDState = BFLS_IDLE;
            ModeA_0();
         }

      }
      else if (blinkingLEDState == BFLS_FEARFUL) {      // User Fearful

          LED3Result r = Mode_Fearful();
          if (r != LED3_FINISH) {
            blinkingLEDState = BFLS_FEARFUL;
          }
          else {
            blinkingLEDState = BFLS_IDLE;
            ModeA_0();
         }

      }
      else if (blinkingLEDState == BFLS_DISGUSTED) {      // User Disgusted

          LED3Result r = Mode_Disgusted();
          if (r != LED3_FINISH) {
            blinkingLEDState = BFLS_DISGUSTED;
          }
          else  {
            blinkingLEDState = BFLS_IDLE;
            ModeA_0();
         }

      }


}

void
BlinkingFaceLED::OpenPrimitives()
{
    for (int i = 0; i < NUM_LEDS; i++) {
        OStatus result = OPENR::OpenPrimitive(LED3_LOCATOR[i], &ledID[i]);
        if (result != oSUCCESS) {
            OSYSLOG1((osyslogERROR, "%s : %s %d",
                      "BlinkingFaceLED::OpenPrimitives()",
                      "OPENR::OpenPrimitive() FAILED", result));
        }
    }
}

void
BlinkingFaceLED::NewCommandVectorData()
{
    OStatus result;
    MemoryRegionID      cmdVecDataID;
    OCommandVectorData* cmdVecData;

    for (int i = 0; i < NUM_COMMAND_VECTOR; i++) {

        result = OPENR::NewCommandVectorData(NUM_LEDS, 
                                             &cmdVecDataID, &cmdVecData);
        if (result != oSUCCESS) {
            OSYSLOG1((osyslogERROR, "%s : %s %d",
                      "BlinkingFaceLED::NewCommandVectorData()",
                      "OPENR::NewCommandVectorData() FAILED", result));
        }

        region[i] = new RCRegion(cmdVecData->vectorInfo.memRegionID,
                                 cmdVecData->vectorInfo.offset,
                                 (void*)cmdVecData,
                                 cmdVecData->vectorInfo.totalSize);

        cmdVecData->SetNumData(NUM_LEDS);

        for (int j = 0; j < NUM_LEDS; j++) {
            OCommandInfo* info = cmdVecData->GetInfo(j);
            info->Set(odataLED_COMMAND3, ledID[j], ocommandMAX_FRAMES);
        }
    }
}


LED3Result
BlinkingFaceLED::Mode_Neutral()
{
    static int level = 0;
    int i;

    RCRegion* rgn = FindFreeRegion();
    OLED3Mode* mode = (OLED3Mode*) calloc(NUM_LEDS, sizeof(OLED3Mode));
    sword* intensity = (sword*) calloc(NUM_LEDS, sizeof(sword));

    mode[11] = oled3_MODE_A;
    intensity[11] = 255;

    for (i = 0; i < NUM_LEDS; i++) {
        if (i != 11) {
            mode[i] = oled3_MODE_A;
            intensity[i] = 0;
        }
    }

    SetLED3Value(rgn, intensity, mode);
    subject[sbjBlink]->SetData(rgn);
    subject[sbjBlink]->NotifyObservers();
    level++;

    free(mode);
    free(intensity);

    if (level > MAX_LEVEL) {
        level = 0;
        return LED3_FINISH;
    } else {
        return LED3_CONT;
    }
}

LED3Result
BlinkingFaceLED::Mode_Joyful()
{
    static int level = 0;
    int i;

    RCRegion* rgn = FindFreeRegion();
    OLED3Mode* mode = (OLED3Mode*) calloc(NUM_LEDS, sizeof(OLED3Mode));
    sword* intensity = (sword*) calloc(NUM_LEDS, sizeof(sword));

    mode[0] = mode[1] = oled3_MODE_B;
    intensity[0] = intensity[1] = 255;

    for (i = 2; i < NUM_LEDS; i++) {
        mode[i] = oled3_MODE_B;
        intensity[i] = 0;
    }

    SetLED3Value(rgn, intensity, mode);
    subject[sbjBlink]->SetData(rgn);
    subject[sbjBlink]->NotifyObservers();
    level++;

    free(mode);
    free(intensity);

    if (level > MAX_LEVEL) {
        level = 0;
        return LED3_FINISH;
    } else {
        return LED3_CONT;
    }
}

LED3Result
BlinkingFaceLED::Mode_Sad()
{
    static int level = 0;
    int i;

    RCRegion* rgn = FindFreeRegion();
    OLED3Mode* mode = (OLED3Mode*) calloc(NUM_LEDS, sizeof(OLED3Mode));
    sword* intensity = (sword*) calloc(NUM_LEDS, sizeof(sword));
    
    mode[12] = oled3_MODE_B;
    intensity[12] = 255;

    for (i = 0; i < NUM_LEDS; i++) {
        if (i != 12) {
            mode[i] = oled3_MODE_B;
            intensity[i] = 0;
        }
    }

     
    SetLED3Value(rgn, intensity, mode);
    subject[sbjBlink]->SetData(rgn);
    subject[sbjBlink]->NotifyObservers();
    level++;
    
    free(mode);
    free(intensity);


    if (level > MAX_LEVEL) {
        level = 0;
        return LED3_FINISH;
    } else {
        return LED3_CONT;
    }
}

LED3Result
BlinkingFaceLED::Mode_Surprised()
{
    static int level = 0;
    int i;

    RCRegion* rgn = FindFreeRegion();
    OLED3Mode* mode = (OLED3Mode*) calloc(NUM_LEDS, sizeof(OLED3Mode));
    sword* intensity = (sword*) calloc(NUM_LEDS, sizeof(sword));
    
    mode[8] = mode[9] = oled3_MODE_A;
    intensity[8] = intensity[9]= 255;

    for (i = 0; i < NUM_LEDS; i++) {
        if (i != 8 && i != 9) {
            mode[i] = oled3_MODE_A;
            intensity[i] = 0;
        }
    }

     
    SetLED3Value(rgn, intensity, mode);
    subject[sbjBlink]->SetData(rgn);
    subject[sbjBlink]->NotifyObservers();
    level++;
    
    free(mode);
    free(intensity);


    if (level > MAX_LEVEL) {
        level = 0;
        return LED3_FINISH;
    } else {
        return LED3_CONT;
    }
}

LED3Result
BlinkingFaceLED::Mode_Angry()
{
    static int level = 0;
    int i;

    RCRegion* rgn = FindFreeRegion();
    OLED3Mode* mode = (OLED3Mode*) calloc(NUM_LEDS, sizeof(OLED3Mode));
    sword* intensity = (sword*) calloc(NUM_LEDS, sizeof(sword));
    
    mode[12] = oled3_MODE_A;
    intensity[12] = 255;

    for (i = 0; i < NUM_LEDS; i++) {
        if (i != 12) {
          mode[i] = oled3_MODE_A;
          intensity[i] = 0;
        }
    }

     
    SetLED3Value(rgn, intensity, mode);
    subject[sbjBlink]->SetData(rgn);
    subject[sbjBlink]->NotifyObservers();
    level++;
    
    free(mode);
    free(intensity);


    if (level > MAX_LEVEL) {
        level = 0;
        return LED3_FINISH;
    } else {
        return LED3_CONT;
    }
}

LED3Result
BlinkingFaceLED::Mode_Fearful()
{
    static int level = 0;
    int i;

    RCRegion* rgn = FindFreeRegion();
    OLED3Mode* mode = (OLED3Mode*) calloc(NUM_LEDS, sizeof(OLED3Mode));
    sword* intensity = (sword*) calloc(NUM_LEDS, sizeof(sword));
    
    
    mode[13] = oled3_MODE_B;
    intensity[13] = 255;

    for (i = 0; i < NUM_LEDS; i++) {
        if (i != 13) {
          mode[i] = oled3_MODE_B;
          intensity[i] = 0;
        }
    }

     
    SetLED3Value(rgn, intensity, mode);
    subject[sbjBlink]->SetData(rgn);
    subject[sbjBlink]->NotifyObservers();
    level++;
    
    free(mode);
    free(intensity);


    if (level > MAX_LEVEL) {
        level = 0;
        return LED3_FINISH;
    } else {
        return LED3_CONT;
    }
}

LED3Result
BlinkingFaceLED::Mode_Disgusted()
{
    static int level = 0;
    int i;

    RCRegion* rgn = FindFreeRegion();
    OLED3Mode* mode = (OLED3Mode*) calloc(NUM_LEDS, sizeof(OLED3Mode));
    sword* intensity = (sword*) calloc(NUM_LEDS, sizeof(sword));

    mode[13] = oled3_MODE_A;
    intensity[13] = 255;

    for (i = 0; i < NUM_LEDS; i++) {
        if (i != 13) {
          mode[i] = oled3_MODE_A;
          intensity[i] = 0;
        }
    }

     
    SetLED3Value(rgn, intensity, mode);
    subject[sbjBlink]->SetData(rgn);
    subject[sbjBlink]->NotifyObservers();
    level++;
    
    free(mode);
    free(intensity);


    if (level > MAX_LEVEL) {
        level = 0;
        return LED3_FINISH;
    } else {
        return LED3_CONT;
    }
}


LED3Result
BlinkingFaceLED::ModeA_0()
{
    static int level = 0;

    RCRegion* rgn = FindFreeRegion();
    SetLED3Value(rgn, 0, oled3_MODE_A);
    subject[sbjBlink]->SetData(rgn);
    subject[sbjBlink]->NotifyObservers();
    level++;

    if (level > MAX_LEVEL) {
        level = 0;
        return LED3_FINISH;
    } else {
        return LED3_CONT;
    }
}

void
BlinkingFaceLED::SetLED3Value(RCRegion* rgn, 
                              int level, int slope, OLED3Mode mode)
{
    OCommandVectorData* cmdVecData = (OCommandVectorData*)rgn->Base();

    for (int i = 0; i < NUM_LEDS; i++) {
        OCommandData* data = cmdVecData->GetData(i);
        OLEDCommandValue3* val = (OLEDCommandValue3*)data->value;
        for (int j = 0; j < ocommandMAX_FRAMES; j++) {
            if (slope == POSITIVE_SLOPE) {
                val[j].intensity = 16 * level + j;
            } else {
                val[j].intensity = 16 * level + 15 - j;
            }
            val[j].mode      = mode;
            val[j].period    = 1;
        }
    }
}

void
BlinkingFaceLED::SetLED3Value(RCRegion* rgn, sword intensity, OLED3Mode mode)
{
    OCommandVectorData* cmdVecData = (OCommandVectorData*)rgn->Base();

    for (int i = 0; i < NUM_LEDS; i++) {
        OCommandData* data = cmdVecData->GetData(i);
        OLEDCommandValue3* val = (OLEDCommandValue3*)data->value;
        for (int j = 0; j < ocommandMAX_FRAMES; j++) {
            val[j].intensity = intensity;
            val[j].mode      = mode;
            val[j].period    = 1;
        }
    }
}

void
BlinkingFaceLED::SetLED3Value(RCRegion* rgn, sword* intensity, OLED3Mode* mode)
{
    OCommandVectorData* cmdVecData = (OCommandVectorData*)rgn->Base();

    for (int i = 0; i < NUM_LEDS; i++) {
        OCommandData* data = cmdVecData->GetData(i);
        OLEDCommandValue3* val = (OLEDCommandValue3*)data->value;
        for (int j = 0; j < ocommandMAX_FRAMES; j++) {
            val[j].intensity = intensity[i];
            val[j].mode      = mode[i];
            val[j].period    = 1;
        }
    }
}


RCRegion*
BlinkingFaceLED::FindFreeRegion()
{
    for (int i = 0; i < NUM_COMMAND_VECTOR; i++) {
        if (region[i]->NumberOfReference() == 1) return region[i];
    }

    return 0;
}

#endif

